From 2dba680fb1431fb96db44a64a06c9fb9c475deed Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sun, 17 Dec 2017 03:18:25 -0500
Subject: [PATCH 01/16] Implemented state and stateMachine classes

---
 .../montclairrobotics/alloy/auto/State.java   |  22 +++
 .../alloy/auto/StateMachine.java              | 128 +++++++++++++++++-
 .../montclairrobotics/alloy/vector/Polar.java |   2 +-
 3 files changed, 144 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/State.java b/src/main/java/org/montclairrobotics/alloy/auto/State.java
index ebb5b45..4352f44 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/State.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/State.java
@@ -15,6 +15,8 @@
 public abstract class State {
 
     Integer nextState = null;
+    String debug = "Running State: ";
+    String description = "None";
 
     /**
      * The start method is the first thing called when the state is run
@@ -37,10 +39,30 @@ public abstract class State {
      */
     public abstract boolean isDone();
 
+    /**
+     * In order to have a non linear state machine, the state machine must know what state
+     * to go to when it is done with the previous state.
+     * The state also sometimes needs to know the current state for example if it just wanted
+     * to increment the state by one.
+     * @param currentState the state the state machine is currently running
+     * @return the state the state machine should go to
+     */
     public int getNextState(int currentState){
         if(nextState != null){
             return nextState;
         }
         return currentState + 1;
     }
+
+    /**
+     * When a state machine is running, it will debug out information about
+     * the state it is running. It will debug the result of debugInfo
+     *
+     * @param currentState the current state so that it can be used in the debug
+     * @return debug information about the state
+     */
+    public String debugInfo(int currentState){
+        return debug + currentState + "\n Description" + description;
+    }
+
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index 6f9cd4d..7af9b1b 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -1,9 +1,12 @@
 package org.montclairrobotics.alloy.auto;
 
+import com.qualcomm.robotcore.util.ElapsedTime;
 import org.montclairrobotics.alloy.core.RobotCore;
 import org.montclairrobotics.alloy.ftc.FTCDebug;
 
+
 import java.util.ArrayList;
+import java.util.Arrays;
 
 /**
  * Created by MHS Robotics on 12/16/2017.
@@ -15,15 +18,20 @@
  * State machines can be used for controlling auto modes,
  * but can also be ran in teleop modes for pre coded instructions that make driving easier.
  *
+ * State machines are also states themselves so a state machine can run another state machine.
+ * This allows for the reuse of auto code
  *
  * @author Garrett Burroughs
- * @since
+ * @since 0.1
+ * @version 0.1
  */
 public class StateMachine extends State {
 
+    /**
+     * The states list keeps track of all of the states that will be run in the state machine
+     */
     ArrayList<State> states;
 
-
     /**
      * Since state machines can run in a non linear fashion, the last state in the array
      * may not be the last state in the state machine, finalState keeps track of what signifies the end
@@ -35,31 +43,137 @@ public class StateMachine extends State {
      */
     Integer finalState;
 
-    public StateMachine(State ... states){
+    /**
+     * The name of the state machine, this is purely for debugging purposes
+     */
+    String name;
+
+    /**
+     * This debug is used for outputting information about the state machine as it runs
+     */
+    FTCDebug d;
+
+    /**
+     * A boolean that keeps track of weather or not the current state has been run yet,
+     * this is used to determine it the start() method of the state should be run
+     */
+    boolean stateStarted;
 
+    /**
+     * the time spent in the state that has most recently finished
+     */
+    double timeInLastState;
+
+    /**
+     * A timer object to keep track of the time in states.
+     */
+    private ElapsedTime timer;
+
+    /**
+     * Keeps track of weather or not the state machine has finished,
+     * True if it has
+     */
+    boolean done = false;
+
+    /**
+     * Keeps track of the current state
+     */
+    int state;
+
+    public StateMachine(String name, int finalState, State ... states){
+        this.name       = name;
+        this.states     = new ArrayList<>(Arrays.asList(states));
+        this.finalState = finalState;
+        d = new FTCDebug(RobotCore.getTelemetry());
+        description = "A state machine";
     }
 
+    public StateMachine(String name, String description, int finalState, State ... states){
+        this(name, finalState, states);
+        super.description = description;
+    }
+
+
+    /**
+     * Read out that the state has started and reset the timer
+     */
     @Override
     public void start() {
-
+        d.log("Running", name);
+        timer.reset();
     }
 
+
+    /**
+     * The run method takes care of actually running the states
+     */
     @Override
     public void run() {
-
+        State currentState = states.get(state);
+
+        //Check If the state has started, if it hasn't run the 'start()' method
+        if (!stateStarted) {
+            timer.reset();
+            currentState.start();
+            stateStarted = true; // State has been started
+        }
+
+        currentState.run(); //run the state
+
+        if(currentState.isDone()){ //check if the state has finished
+            timeInLastState = timer.nanoseconds(); // Update the last state time
+            currentState.stop(); // Run the stop() method on the state
+            if(currentState.getNextState(state) == finalState){ // Check if the state machine is done
+                done = true; // set the state machine as done
+                return; // return (exit) out of the run() method
+            }
+            if(currentState.getNextState(state) < states.size()) { // make sure there is a next state to go to
+                state = currentState.getNextState(state); // go to the next state
+            }else{
+                d.log("ERROR", "STATE MACHINE OUT OF BOUNDS"); // Give the user an error if there is no next state to go to
+                done = true; // stop the state machine
+                return; // exit the run method to ensure nothing else runs
+            }
+            stateStarted = false; // The next state has not started yet.
+        }
+        d.msg(currentState.debugInfo(state)); // Debug info about the state
     }
 
+
+    /**
+     * When the state machine is finished, read out it has finished
+     */
     @Override
     public void stop() {
-
+        d.log("Status", name + ", Finished ");
     }
 
+    /**
+     * determine if the state machine is done
+     *
+     * @return true if the state machine is done
+     */
     @Override
     public boolean isDone() {
-        return false;
+        return done;
     }
 
+    /**
+     * Adds a state to the state machine.
+     * States can be added to a state machine but should not be added after the state machine
+     *
+     * @param state state that will be added to the state machine
+     */
     public void addState(State state){
         states.add(state);
     }
+
+    /**
+     * Gets the time in the most recently completed state
+     *
+     * @return The time in last state
+     */
+    public double getTimeInLastState(){
+        return timeInLastState;
+    }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/vector/Polar.java b/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
index 56f0a99..4d6989b 100644
--- a/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
+++ b/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
@@ -21,7 +21,7 @@ public class Polar implements Vector {
 
     public Polar(double magnitude, Angle angle) {
         this.magnitude = magnitude;
-        this.angle = angle;
+        this.angle     = angle;
     }
 
     /**

From 00ae340c3df9549d29921ad1b1ba0f4da929dadd Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sun, 17 Dec 2017 03:36:46 -0500
Subject: [PATCH 02/16] Implemented alloy autonomous

---
 .../alloy/auto/AlloyAutonomous.java           | 50 ++++++++++++++++++-
 1 file changed, 49 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index 89f62e4..cec0cab 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -1,6 +1,8 @@
 package org.montclairrobotics.alloy.auto;
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.montclairrobotics.alloy.core.RobotCore;
 
 /**
  * Created by MHS Robotics on 12/5/2017.
@@ -8,15 +10,61 @@
  * @author Garrett Burroughs
  */
 public abstract class AlloyAutonomous extends OpMode{
+    /**
+     * Keeps track of when the auto mode is running (True after started and before finished)
+     */
+    boolean running;
 
+    /**
+     * The actual auto mode that should be instantiated in setup
+     */
+    public StateMachine auto;
 
+    /**
+     * A timer to keep track of time in the autoMode
+     */
+    public ElapsedTime timer;
+
+
+    /**
+     * This is where the user should define all their code
+     * and where "Auto", should be instantiated
+     */
+    public abstract void setup();
+
+    /**
+     * Runs when the play button is pressed
+     * Start will set up everything that the auto mode needs to run
+     */
+    @Override
+    public void start(){
+        auto.start(); // run the start method of the state machine
+        running = true; // the state machine has started running
+        timer.reset(); // reset the timer
+    }
+
+    /**
+     * Init is called when the INIT button is pressed on the drivers station.
+     * The init method takes care of setting up global robot variables and running the user setup method
+     */
     @Override
     public void init() {
+        new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
+        setup();
 
     }
 
+    /**
+     * the Loop method takes care of running the state machine
+     */
     @Override
     public void loop() {
-
+        if(running){
+            auto.run(); // Run the state machine, which will in turn run the states
+        }
+        if(auto.isDone()){ // check if the state machine has finished (Last state achieved)
+            running = false; // stop the state machine
+            auto.stop(); // Finally stop the state machine
+        }
     }
 }

From 4de9925032a69c2668dcc08bbe188a73a0766624 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sun, 17 Dec 2017 03:38:34 -0500
Subject: [PATCH 03/16] Added newState method in autostatmachine

---
 .../montclairrobotics/alloy/auto/AlloyAutonomous.java    | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index cec0cab..90c370d 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -67,4 +67,13 @@ public void loop() {
             auto.stop(); // Finally stop the state machine
         }
     }
+
+    /**
+     * A wrapper around the add state method to allow the user to add states to the auto mode
+     *
+     * @param state the state to be added
+     */
+    public void newState(State state){
+        auto.addState(state);
+    }
 }

From 6dd957ce8bfffc074bb95cdce6bb08d0a7aa3fb0 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Fri, 26 Jan 2018 09:54:42 -0500
Subject: [PATCH 04/16] Update FTCDebug.java

---
 .../org/montclairrobotics/alloy/ftc/FTCDebug.java  | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
index 47ed463..796993b 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
@@ -8,19 +8,19 @@
  */
 public class FTCDebug implements Debug {
 
-    Telemetry telemetry;
+    public static Telemetry telemetry;
 
-    public FTCDebug(Telemetry telemetry){
-        this.telemetry = telemetry;
+    public FTCDebug(){
+        this.telemetry = RobotCore.getTelemetry();
     }
 
     @Override
-    public void log(String key, Object value) {
-
+    public static void log(String key, Object value) {
+        telemetry.addData(key, object);
     }
 
     @Override
-    public void msg(Object value) {
-
+    public static void msg(Object value) {
+        telemetry.addData("Debug", value);
     }
 }

From 54874bb84e5c9b1f8819af42b91dce90bd5792e2 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Fri, 26 Jan 2018 09:55:28 -0500
Subject: [PATCH 05/16] Created static init() method for Debug.java

---
 src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
index 796993b..cc03ed5 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
@@ -10,7 +10,7 @@ public class FTCDebug implements Debug {
 
     public static Telemetry telemetry;
 
-    public FTCDebug(){
+    public static init(){
         this.telemetry = RobotCore.getTelemetry();
     }
 

From 2a40fd5a0c5290cf44881939eaaccb104401564b Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Fri, 26 Jan 2018 09:56:47 -0500
Subject: [PATCH 06/16] Initialize FTCDebug

---
 src/main/java/org/montclairrobotics/alloy/core/Alloy.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
index 4c4a312..b90357d 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
@@ -36,6 +36,7 @@ public void init() {
         // Set Up the core robot components, This allows them to be accessed throughout the project
         new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
         initialization();
+        FTCDebug.init();
     }
 
     @Override

From ee4f1cd48fceda1fe4ecc17d945b1333344373bb Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sat, 27 Jan 2018 00:04:56 -0500
Subject: [PATCH 07/16] Implemented base alloy autonomous

---
 .idea/misc.xml                                |  2 +-
 .../Exceptions/InvalidParameterException.java | 10 ++++++
 .../alloy/auto/AlloyAutonomous.java           | 21 ++++++++++-
 .../alloy/auto/SimpleAutonomous.java          | 32 +++++++++++++++++
 .../alloy/auto/States/Drive.java              | 36 +++++++++++++++++++
 .../alloy/auto/States/Turn.java               | 36 +++++++++++++++++++
 .../montclairrobotics/alloy/core/Alloy.java   |  2 +-
 7 files changed, 136 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
 create mode 100644 src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
 create mode 100644 src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
 create mode 100644 src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java

diff --git a/.idea/misc.xml b/.idea/misc.xml
index cfcd7a9..5a8ced9 100644
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -2,7 +2,7 @@
 <project version="4">
   <component name="JavadocGenerationManager">
     <option name="OUTPUT_DIRECTORY" value="$USER_HOME$/Desktop/Files" />
-    <option name="OPTION_SCOPE" value="protected" />
+    <option name="OPTION_SCOPE" value="public" />
     <option name="OPTION_HIERARCHY" value="true" />
     <option name="OPTION_NAVIGATOR" value="true" />
     <option name="OPTION_INDEX" value="true" />
diff --git a/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java b/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
new file mode 100644
index 0000000..9ba20e9
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
@@ -0,0 +1,10 @@
+package org.montclairrobotics.alloy.Exceptions;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class InvalidParameterException extends Exception{
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index 90c370d..754b69f 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -7,7 +7,18 @@
 /**
  * Created by MHS Robotics on 12/5/2017.
  *
+ * AlloyAutonomous is the core framework for coding auto modes
+ * When an auto mode extends AlloyAutonomous, it needs to override
+ * the setup() method, and has the option of overriding the userLoop()
+ * method. In the setup method, <code>auto<code/> needs to be set to a
+ * state machine, that will be run during the auto mode
+ *
+ * The optional userLoop() method can be used to update or perform
+ * actions every loop, it is called before the state in the state machine
+ *
  * @author Garrett Burroughs
+ * @version 0.1
+ * @since 0.1
  */
 public abstract class AlloyAutonomous extends OpMode{
     /**
@@ -53,12 +64,20 @@ public void init() {
         setup();
 
     }
-
+    
+    /**
+     * The user loop can be overridden in the auto mode and is called every loop before the
+     * state is run. This can be useful for updating information used in states.
+     */
+    public void userLoop(){}
+    
+    
     /**
      * the Loop method takes care of running the state machine
      */
     @Override
     public void loop() {
+        userLoop();
         if(running){
             auto.run(); // Run the state machine, which will in turn run the states
         }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
new file mode 100644
index 0000000..ea16c2f
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
@@ -0,0 +1,32 @@
+package org.montclairrobotics.alloy.auto;
+
+import org.montclairrobotics.alloy.auto.States.Drive;
+import org.montclairrobotics.alloy.auto.States.Turn;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * SimpleAutonomous abstracts away the concept of
+ * adding states to a state machine, so that states
+ * can be added as if they were commands to be executed
+ * by the auto mode. This class only has the pre-written
+ * states included, so another class extending this one
+ * would need to be created to add any custom states
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class SimpleAutonomous extends AlloyAutonomous{
+    @Override
+    public void setup() {
+    
+    }
+    
+    public void drive(double speed, double distance){
+        auto.addState(new Drive(speed, distance));
+    }
+    
+    public void turn(double speed, double degrees){
+        auto.addState(new Turn(speed, degrees));
+    }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
new file mode 100644
index 0000000..7d2af04
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
@@ -0,0 +1,36 @@
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class Drive extends State{
+    
+    public Drive(double speed, double distance){
+    
+    }
+    
+    @Override
+    public void start() {
+    
+    }
+    
+    @Override
+    public void run() {
+    
+    }
+    
+    @Override
+    public void stop() {
+    
+    }
+    
+    @Override
+    public boolean isDone() {
+        return false;
+    }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
new file mode 100644
index 0000000..adc5ae0
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
@@ -0,0 +1,36 @@
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class Turn extends State {
+    
+    public Turn(double speed, double degrees){
+    
+    }
+    
+    @Override
+    public void start() {
+    
+    }
+    
+    @Override
+    public void run() {
+    
+    }
+    
+    @Override
+    public void stop() {
+    
+    }
+    
+    @Override
+    public boolean isDone() {
+        return false;
+    }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
index 4c4a312..0d0d7f1 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
@@ -5,7 +5,7 @@
 /**
  * Created by MHS Robotics on 11/13/2017.
  *
- * The main purpose behind the alloy class is to controll how and when all of the mehotds in Robot are run
+ * The main purpose behind the alloy class is to control how and when all of the methods in Robot are run
  * as well as initialize all global variables
  *
  * @author Garrett Burroughs

From a133fd2ebed2ede9072284719fc6eae7ea12f2f7 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sat, 27 Jan 2018 00:09:14 -0500
Subject: [PATCH 08/16] removed files that should have been ignored

---
 .gitignore         |  2 ++
 .idea/compiler.xml | 17 -----------------
 .idea/misc.xml     | 31 -------------------------------
 .idea/modules.xml  |  8 --------
 .idea/vcs.xml      |  6 ------
 .travis.yml        |  6 +++---
 README.md          | 16 ++++++++--------
 7 files changed, 13 insertions(+), 73 deletions(-)
 delete mode 100644 .idea/compiler.xml
 delete mode 100644 .idea/misc.xml
 delete mode 100644 .idea/modules.xml
 delete mode 100644 .idea/vcs.xml

diff --git a/.gitignore b/.gitignore
index 998c1fa..6fe0f66 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,6 +5,8 @@
 .idea/**/workspace.xml
 .idea/**/tasks.xml
 .idea/dictionaries
+.idea/
+main/java/main.iml
 
 # Sensitive or high-churn files:
 .idea/**/dataSources/
diff --git a/.idea/compiler.xml b/.idea/compiler.xml
deleted file mode 100644
index 085bb29..0000000
--- a/.idea/compiler.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="CompilerConfiguration">
-    <annotationProcessing>
-      <profile name="Maven default annotation processors profile" enabled="true">
-        <sourceOutputDir name="target/generated-sources/annotations" />
-        <sourceTestOutputDir name="target/generated-test-sources/test-annotations" />
-        <outputRelativeToContentRoot value="true" />
-        <module name="main" />
-      </profile>
-    </annotationProcessing>
-    <bytecodeTargetLevel>
-      <module name="Alloy" target="1.5" />
-      <module name="main" target="1.7" />
-    </bytecodeTargetLevel>
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
deleted file mode 100644
index 5a8ced9..0000000
--- a/.idea/misc.xml
+++ /dev/null
@@ -1,31 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="JavadocGenerationManager">
-    <option name="OUTPUT_DIRECTORY" value="$USER_HOME$/Desktop/Files" />
-    <option name="OPTION_SCOPE" value="public" />
-    <option name="OPTION_HIERARCHY" value="true" />
-    <option name="OPTION_NAVIGATOR" value="true" />
-    <option name="OPTION_INDEX" value="true" />
-    <option name="OPTION_SEPARATE_INDEX" value="true" />
-    <option name="OPTION_DOCUMENT_TAG_USE" value="true" />
-    <option name="OPTION_DOCUMENT_TAG_AUTHOR" value="true" />
-    <option name="OPTION_DOCUMENT_TAG_VERSION" value="true" />
-    <option name="OPTION_DOCUMENT_TAG_DEPRECATED" value="true" />
-    <option name="OPTION_DEPRECATED_LIST" value="true" />
-    <option name="OTHER_OPTIONS" />
-    <option name="HEAP_SIZE" />
-    <option name="LOCALE" />
-    <option name="OPEN_IN_BROWSER" value="true" />
-    <option name="OPTION_INCLUDE_LIBS" value="false" />
-  </component>
-  <component name="MavenProjectsManager">
-    <option name="originalFiles">
-      <list>
-        <option value="$PROJECT_DIR$/src/main/pom.xml" />
-      </list>
-    </option>
-  </component>
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" default="true" project-jdk-name="1.7" project-jdk-type="JavaSDK">
-    <output url="file://$PROJECT_DIR$/out" />
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
deleted file mode 100644
index 95e4de6..0000000
--- a/.idea/modules.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/src/main/main.iml" filepath="$PROJECT_DIR$/src/main/main.iml" />
-    </modules>
-  </component>
-</project>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
deleted file mode 100644
index 35eb1dd..0000000
--- a/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="" vcs="Git" />
-  </component>
-</project>
\ No newline at end of file
diff --git a/.travis.yml b/.travis.yml
index ee6f970..9000af6 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,3 +1,3 @@
-language: java
-jdk: 
-- oraclejdk8
+language: java
+jdk: 
+- oraclejdk8
diff --git a/README.md b/README.md
index 82241a7..85e1d50 100644
--- a/README.md
+++ b/README.md
@@ -1,8 +1,8 @@
-# Alloy  [![Build Status](https://travis-ci.org/GarrettBurroughs/Alloy.svg?branch=master)](https://travis-ci.org/GarrettBurroughs/Alloy)
-
-Status: **Beta**
-
-
-Alloy is a robot framework designed specifically for the First Tech Challenge (FTC).
-
-Alloy is based off the FRC robot framework [Sprocket](https://github.com/MontclairRobotics/Sprocket), but redesigned to abstract away the more complex ideas but still allow for advanced robot functionality.
+# Alloy  [![Build Status](https://travis-ci.org/GarrettBurroughs/Alloy.svg?branch=master)](https://travis-ci.org/GarrettBurroughs/Alloy)
+
+Status: **Beta**
+
+
+Alloy is a robot framework designed specifically for the First Tech Challenge (FTC).
+
+Alloy is based off the FRC robot framework [Sprocket](https://github.com/MontclairRobotics/Sprocket), but redesigned to abstract away the more complex ideas but still allow for advanced robot functionality.

From 601a4a8acfc068913220124702ea43eaf472aa4e Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Fri, 2 Feb 2018 22:41:23 -0500
Subject: [PATCH 09/16] revert changes that made debug static

---
 .../org/montclairrobotics/alloy/auto/StateMachine.java   | 2 +-
 .../java/org/montclairrobotics/alloy/core/Alloy.java     | 1 -
 .../java/org/montclairrobotics/alloy/ftc/FTCDebug.java   | 9 +++++----
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index 7af9b1b..4036d13 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -84,7 +84,7 @@ public StateMachine(String name, int finalState, State ... states){
         this.name       = name;
         this.states     = new ArrayList<>(Arrays.asList(states));
         this.finalState = finalState;
-        d = new FTCDebug(RobotCore.getTelemetry());
+        d = new FTCDebug();
         description = "A state machine";
     }
 
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
index cb164f7..36d9b7b 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
@@ -45,7 +45,6 @@ public void init() {
         new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
         robotSetup();
         initialization();
-        FTCDebug.init();
     }
 
     @Override
diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
index cc03ed5..1f497e6 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCDebug.java
@@ -2,6 +2,7 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.montclairrobotics.alloy.core.Debug;
+import org.montclairrobotics.alloy.core.RobotCore;
 
 /**
  * Created by Garrett on 11/14/2017.
@@ -10,17 +11,17 @@ public class FTCDebug implements Debug {
 
     public static Telemetry telemetry;
 
-    public static init(){
+    public FTCDebug(){
         this.telemetry = RobotCore.getTelemetry();
     }
 
     @Override
-    public static void log(String key, Object value) {
-        telemetry.addData(key, object);
+    public void log(String key, Object value) {
+        telemetry.addData(key, value);
     }
 
     @Override
-    public static void msg(Object value) {
+    public void msg(Object value) {
         telemetry.addData("Debug", value);
     }
 }

From 41548194b16782d498a950b54dbf72ffa61370c2 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrettburroughs02@gmail.com>
Date: Sun, 11 Feb 2018 20:03:45 -0500
Subject: [PATCH 10/16] Readme Update

---
 src/main/java/org/montclairrobotics/alloy/auto/README.md | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/README.md b/src/main/java/org/montclairrobotics/alloy/auto/README.md
index 41048d5..e0b65f2 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/README.md
+++ b/src/main/java/org/montclairrobotics/alloy/auto/README.md
@@ -16,4 +16,7 @@ when they have finished running
 
 3. Alloy Autonomus - This is the basic framework for alloy auto's. An alloy autonomous takes care of running the state machine<br>
 that the auto is using so all the user has to do is create the state machine and define functionality.
-You can read more about how to use an Alloy Autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
\ No newline at end of file
+You can read more about how to use an Alloy Autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
+
+4. Simple Autonomous - A simple autonomous mode further abstracts away auto mode creation allowing 
+for command like auto mode programming. You can read more about how to use a Simple autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
\ No newline at end of file

From e4b7785184dd2fd8ecf558a252c5899f7bdf1098 Mon Sep 17 00:00:00 2001
From: GarrettBurroughs <garrettburroughs02@gmail.com>
Date: Wed, 12 Sep 2018 00:50:00 -0400
Subject: [PATCH 11/16] Implemented turning and driving auto modes

---
 build.gradle                                  |  2 +-
 .../Exceptions/InvalidParameterException.java | 26 ++++-
 .../alloy/auto/AlloyAutonomous.java           | 49 ++++------
 .../alloy/auto/SimpleAutonomous.java          | 47 +++++++---
 .../montclairrobotics/alloy/auto/State.java   | 19 ++--
 .../alloy/auto/StateMachine.java              | 94 ++++++++-----------
 .../alloy/auto/States/Drive.java              | 78 ++++++++++++---
 .../alloy/auto/States/Turn.java               | 92 +++++++++++++++---
 .../montclairrobotics/alloy/core/Alloy.java   |  6 +-
 .../alloy/core/Debugger.java                  |  1 -
 .../montclairrobotics/alloy/core/Encoder.java | 15 ++-
 .../alloy/drive/DriveTrain.java               | 57 +++++++++--
 .../alloy/drive/TankDrive.java                |  2 +-
 .../montclairrobotics/alloy/ftc/FTCMotor.java |  2 +-
 .../alloy/ftc/FTCTargetMotor.java             |  2 +-
 .../alloy/motor/MotorGroup.java               | 32 +++++--
 .../alloy/motor/MotorModule.java              | 85 ++++++++++++++---
 .../alloy/test/IntakeMapper.java              |  6 +-
 .../alloy/test/TestRobot.java                 | 26 +++--
 .../montclairrobotics/alloy/vector/Polar.java |  2 +-
 20 files changed, 457 insertions(+), 186 deletions(-)

diff --git a/build.gradle b/build.gradle
index 265a14a..1089740 100644
--- a/build.gradle
+++ b/build.gradle
@@ -29,7 +29,7 @@ spotless {
 
 
         licenseHeaderFile 'SpotlessLicense'
-		paddedCell()
+		// paddedCell()
 		
 
     }
diff --git a/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java b/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
index 9ba20e9..275c9a5 100644
--- a/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
+++ b/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
@@ -1,3 +1,26 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.Exceptions;
 
 /**
@@ -6,5 +29,4 @@
  * @author Garrett Burroughs
  * @since
  */
-public class InvalidParameterException extends Exception{
-}
+public class InvalidParameterException extends Exception {}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index 92a284a..dc852a6 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -43,67 +43,56 @@ of this software and associated documentation files (the "Software"), to deal
  * @version 0.1
  * @since 0.1
  */
-public abstract class AlloyAutonomous extends OpMode{
-    /**
-     * Keeps track of when the auto mode is running (True after started and before finished)
-     */
+public abstract class AlloyAutonomous extends OpMode {
+    /** Keeps track of when the auto mode is running (True after started and before finished) */
     boolean running;
 
-    /**
-     * The actual auto mode that should be instantiated in setup
-     */
+    /** The actual auto mode that should be instantiated in setup */
     public StateMachine auto;
 
-    /**
-     * A timer to keep track of time in the autoMode
-     */
+    /** A timer to keep track of time in the autoMode */
     public ElapsedTime timer;
 
-
     /**
-     * This is where the user should define all their code
-     * and where "Auto", should be instantiated
+     * This is where the user should define all their code and where "Auto", should be instantiated
      */
     public abstract void setup();
 
     /**
-     * Runs when the play button is pressed
-     * Start will set up everything that the auto mode needs to run
+     * Runs when the play button is pressed Start will set up everything that the auto mode needs to
+     * run
      */
     @Override
-    public void start(){
+    public void start() {
         auto.start(); // run the start method of the state machine
         running = true; // the state machine has started running
         timer.reset(); // reset the timer
     }
 
     /**
-     * Init is called when the INIT button is pressed on the drivers station.
-     * The init method takes care of setting up global robot variables and running the user setup method
+     * Init is called when the INIT button is pressed on the drivers station. The init method takes
+     * care of setting up global robot variables and running the user setup method
      */
     @Override
     public void init() {
         new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
         setup();
     }
-    
-    /**
-     * The user loop can be overridden in the auto mode and is called every loop before the
-     * state is run. This can be useful for updating information used in states.
-     */
-    public void userLoop(){}
-    
-    
+
     /**
-     * the Loop method takes care of running the state machine
+     * The user loop can be overridden in the auto mode and is called every loop before the state is
+     * run. This can be useful for updating information used in states.
      */
+    public void userLoop() {}
+
+    /** the Loop method takes care of running the state machine */
     @Override
     public void loop() {
         userLoop();
-        if(running){
+        if (running) {
             auto.run(); // Run the state machine, which will in turn run the states
         }
-        if(auto.isDone()){ // check if the state machine has finished (Last state achieved)
+        if (auto.isDone()) { // check if the state machine has finished (Last state achieved)
             running = false; // stop the state machine
             auto.stop(); // Finally stop the state machine
         }
@@ -114,7 +103,7 @@ public void loop() {
      *
      * @param state the state to be added
      */
-    public void newState(State state){
+    public void newState(State state) {
         auto.addState(state);
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
index ea16c2f..a45f712 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
@@ -1,3 +1,26 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.auto;
 
 import org.montclairrobotics.alloy.auto.States.Drive;
@@ -6,27 +29,23 @@
 /**
  * Created by MHS Robotics on 1/26/2018.
  *
- * SimpleAutonomous abstracts away the concept of
- * adding states to a state machine, so that states
- * can be added as if they were commands to be executed
- * by the auto mode. This class only has the pre-written
- * states included, so another class extending this one
- * would need to be created to add any custom states
+ * <p>SimpleAutonomous abstracts away the concept of adding states to a state machine, so that
+ * states can be added as if they were commands to be executed by the auto mode. This class only has
+ * the pre-written states included, so another class extending this one would need to be created to
+ * add any custom states
  *
  * @author Garrett Burroughs
  * @since
  */
-public class SimpleAutonomous extends AlloyAutonomous{
+public class SimpleAutonomous extends AlloyAutonomous {
     @Override
-    public void setup() {
-    
-    }
-    
-    public void drive(double speed, double distance){
+    public void setup() {}
+
+    public void drive(double speed, double distance) {
         auto.addState(new Drive(speed, distance));
     }
-    
-    public void turn(double speed, double degrees){
+
+    public void turn(double speed, double degrees) {
         auto.addState(new Turn(speed, degrees));
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/State.java b/src/main/java/org/montclairrobotics/alloy/auto/State.java
index 30feedb..5035059 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/State.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/State.java
@@ -58,29 +58,28 @@ public abstract class State {
     public abstract boolean isDone();
 
     /**
-     * In order to have a non linear state machine, the state machine must know what state
-     * to go to when it is done with the previous state.
-     * The state also sometimes needs to know the current state for example if it just wanted
-     * to increment the state by one.
+     * In order to have a non linear state machine, the state machine must know what state to go to
+     * when it is done with the previous state. The state also sometimes needs to know the current
+     * state for example if it just wanted to increment the state by one.
+     *
      * @param currentState the state the state machine is currently running
      * @return the state the state machine should go to
      */
-    public int getNextState(int currentState){
-        if(nextState != null){
+    public int getNextState(int currentState) {
+        if (nextState != null) {
             return nextState;
         }
         return currentState + 1;
     }
 
     /**
-     * When a state machine is running, it will debug out information about
-     * the state it is running. It will debug the result of debugInfo
+     * When a state machine is running, it will debug out information about the state it is running.
+     * It will debug the result of debugInfo
      *
      * @param currentState the current state so that it can be used in the debug
      * @return debug information about the state
      */
-    public String debugInfo(int currentState){
+    public String debugInfo(int currentState) {
         return debug + currentState + "\n Description" + description;
     }
-
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index 3bff746..8048121 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -24,22 +24,21 @@ of this software and associated documentation files (the "Software"), to deal
 package org.montclairrobotics.alloy.auto;
 
 import com.qualcomm.robotcore.util.ElapsedTime;
-import org.montclairrobotics.alloy.components.Component;
-
 import java.util.ArrayList;
 import java.util.Arrays;
+import org.montclairrobotics.alloy.components.Component;
 
 /**
  * Created by MHS Robotics on 12/16/2017.
  *
- * A state machine takes in states and runs them in a controlled order.<br />
- * By default the states will run in a linear fashion and the next state will start
- * when the previous state has finished.<br />
- * <br />
- * State machines can be used for controlling auto modes,
- * but can also be ran in teleop modes for pre coded instructions that make driving easier.
+ * <p>A state machine takes in states and runs them in a controlled order.<br>
+ * By default the states will run in a linear fashion and the next state will start when the
+ * previous state has finished.<br>
+ * <br>
+ * State machines can be used for controlling auto modes, but can also be ran in teleop modes for
+ * pre coded instructions that make driving easier.
  *
- * State machines are also states themselves so a state machine can run another state machine.
+ * <p>State machines are also states themselves so a state machine can run another state machine.
  * This allows for the reuse of auto code
  *
  * @author Garrett Burroughs
@@ -48,9 +47,7 @@ of this software and associated documentation files (the "Software"), to deal
  */
 public class StateMachine extends State {
 
-    /**
-     * The states list keeps track of all of the states that will be run in the state machine
-     */
+    /** The states list keeps track of all of the states that will be run in the state machine */
     ArrayList<State> states;
 
     /**
@@ -66,64 +63,47 @@ public class StateMachine extends State {
      */
     Integer finalState;
 
-    /**
-     * The name of the state machine, this is purely for debugging purposes
-     */
+    /** The name of the state machine, this is purely for debugging purposes */
     String name;
 
     /**
-     * A boolean that keeps track of weather or not the current state has been run yet,
-     * this is used to determine it the start() method of the state should be run
+     * A boolean that keeps track of weather or not the current state has been run yet, this is used
+     * to determine it the start() method of the state should be run
      */
     boolean stateStarted;
 
-    /**
-     * the time spent in the state that has most recently finished
-     */
+    /** the time spent in the state that has most recently finished */
     double timeInLastState;
 
-    /**
-     * A timer object to keep track of the time in states.
-     */
+    /** A timer object to keep track of the time in states. */
     private ElapsedTime timer;
 
-    /**
-     * Keeps track of weather or not the state machine has finished,
-     * True if it has
-     */
+    /** Keeps track of weather or not the state machine has finished, True if it has */
     boolean done = false;
 
-    /**
-     * Keeps track of the current state
-     */
+    /** Keeps track of the current state */
     int state;
 
-    public StateMachine(String name, int finalState, State ... states){
-        this.name       = name;
-        this.states     = new ArrayList<>(Arrays.asList(states));
+    public StateMachine(String name, int finalState, State... states) {
+        this.name = name;
+        this.states = new ArrayList<>(Arrays.asList(states));
         this.finalState = finalState;
         description = "A state machine";
     }
 
-    public StateMachine(String name, String description, int finalState, State ... states){
+    public StateMachine(String name, String description, int finalState, State... states) {
         this(name, finalState, states);
         super.description = description;
     }
 
-
-    /**
-     * Read out that the state has started and reset the timer
-     */
+    /** Read out that the state has started and reset the timer */
     @Override
     public void start() {
         Component.debugger.driverInfo("Running", name);
         timer.reset();
     }
 
-
-    /**
-     * The run method takes care of actually running the states
-     */
+    /** The run method takes care of actually running the states */
     @Override
     public void run() {
         State currentState = states.get(state);
@@ -137,29 +117,31 @@ public void run() {
 
         currentState.run(); //run the state
 
-        if(currentState.isDone()){ //check if the state has finished
+        if (currentState.isDone()) { //check if the state has finished
             timeInLastState = timer.nanoseconds(); // Update the last state time
             currentState.stop(); // Run the stop() method on the state
-            if(currentState.getNextState(state) == finalState){ // Check if the state machine is done
+            if (currentState.getNextState(state)
+                    == finalState) { // Check if the state machine is done
                 done = true; // set the state machine as done
                 return; // return (exit) out of the run() method
             }
-            if(currentState.getNextState(state) < states.size()) { // make sure there is a next state to go to
+            if (currentState.getNextState(state)
+                    < states.size()) { // make sure there is a next state to go to
                 state = currentState.getNextState(state); // go to the next state
-            }else{
-                Component.debugger.error( "STATE MACHINE OUT OF BOUNDS"); // Give the user an error if there is no next state to go to
+            } else {
+                Component.debugger.error(
+                        "STATE MACHINE OUT OF BOUNDS"); // Give the user an error if there is no next state to go to
                 done = true; // stop the state machine
                 return; // exit the run method to ensure nothing else runs
             }
             stateStarted = false; // The next state has not started yet.
         }
-        Component.debugger.test(name + " | State: " + state ,currentState.debugInfo(state)); // Debug info about the state
+        Component.debugger.test(
+                name + " | State: " + state,
+                currentState.debugInfo(state)); // Debug info about the state
     }
 
-
-    /**
-     * When the state machine is finished, read out it has finished
-     */
+    /** When the state machine is finished, read out it has finished */
     @Override
     public void stop() {
         Component.debugger.test("Status", name + ", Finished ");
@@ -176,12 +158,12 @@ public boolean isDone() {
     }
 
     /**
-     * Adds a state to the state machine.
-     * States can be added to a state machine but should not be added after the state machine
+     * Adds a state to the state machine. States can be added to a state machine but should not be
+     * added after the state machine
      *
      * @param state state that will be added to the state machine
      */
-    public void addState(State state){
+    public void addState(State state) {
         states.add(state);
     }
 
@@ -190,7 +172,7 @@ public void addState(State state){
      *
      * @return The time in last state
      */
-    public double getTimeInLastState(){
+    public double getTimeInLastState() {
         return timeInLastState;
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
index 7d2af04..9d0e114 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
@@ -1,6 +1,36 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.auto.States;
 
 import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.core.Alloy;
+import org.montclairrobotics.alloy.drive.DTInput;
+import org.montclairrobotics.alloy.drive.DriveTrain;
+import org.montclairrobotics.alloy.utils.ConstantInput;
+import org.montclairrobotics.alloy.vector.Angle;
+import org.montclairrobotics.alloy.vector.Polar;
+import org.montclairrobotics.alloy.vector.Vector;
 
 /**
  * Created by MHS Robotics on 1/26/2018.
@@ -8,29 +38,53 @@
  * @author Garrett Burroughs
  * @since
  */
-public class Drive extends State{
-    
-    public Drive(double speed, double distance){
-    
+public class Drive extends State {
+
+    DriveTrain driveTrain;
+    private double speed;
+    private double distance;
+    private int[] startValues;
+    private static int tolerance = 5;
+
+    public static void setTolerance(int tolerance) {
+        Drive.tolerance = tolerance;
+    }
+
+    public Drive(double speed, double distance) {
+        driveTrain = Alloy.getDriveTrain();
+        this.speed = speed;
+        this.distance = distance;
+    }
+
+    public Drive setDriveTrain(DriveTrain driveTrain) {
+        this.driveTrain = driveTrain;
+        return this;
     }
-    
+
     @Override
     public void start() {
-    
+        startValues = driveTrain.getEncoderValues();
     }
-    
+
     @Override
     public void run() {
-    
+        driveTrain.setInput(
+                new ConstantInput<DTInput>(new DTInput(new Polar(speed, Angle.ZERO), Angle.ZERO)));
     }
-    
+
     @Override
     public void stop() {
-    
+        driveTrain.setInput(new ConstantInput<DTInput>(new DTInput(Vector.ZERO, Angle.ZERO)));
     }
-    
+
     @Override
     public boolean isDone() {
-        return false;
+        int[] currentValues = driveTrain.getEncoderValues();
+        double total = 0;
+        for (int i = 0; i < startValues.length; i++) {
+            total += currentValues[i] - startValues[i];
+        }
+        double average = total / (double) currentValues.length;
+        return Math.abs(distance - average) < tolerance;
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
index adc5ae0..fc260af 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
@@ -1,6 +1,35 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.auto.States;
 
 import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.core.Alloy;
+import org.montclairrobotics.alloy.drive.DTInput;
+import org.montclairrobotics.alloy.drive.DriveTrain;
+import org.montclairrobotics.alloy.utils.ConstantInput;
+import org.montclairrobotics.alloy.vector.Angle;
+import org.montclairrobotics.alloy.vector.Vector;
 
 /**
  * Created by MHS Robotics on 1/26/2018.
@@ -9,28 +38,69 @@
  * @since
  */
 public class Turn extends State {
-    
-    public Turn(double speed, double degrees){
-    
+
+    private double speed;
+    private double degrees;
+    private int[] rightSideStartValues;
+    private int[] leftSideStartValues;
+    private DriveTrain driveTrain;
+
+    private static double ticksPerDegree = 1;
+
+    public static void setTicksPerDegree(double ticksPerDegree) {
+        Turn.ticksPerDegree = ticksPerDegree;
+    }
+
+    private static double tolerance = 5;
+
+    public static void setTolerance(double tolerance) {
+        Turn.tolerance = tolerance;
     }
-    
+
+    public Turn(double speed, double degrees) {
+        this.speed = speed;
+        this.degrees = degrees;
+        driveTrain = Alloy.getDriveTrain();
+    }
+
     @Override
     public void start() {
-    
+        rightSideStartValues = Alloy.getDriveTrain().getRightEncoderValues();
+        leftSideStartValues = Alloy.getDriveTrain().getLeftEncoderValues();
     }
-    
+
     @Override
     public void run() {
-    
+        if (degrees > 0) {
+            driveTrain.setInput(
+                    new ConstantInput<DTInput>(new DTInput(Vector.ZERO, new Angle(speed))));
+        } else {
+            driveTrain.setInput(
+                    new ConstantInput<DTInput>(new DTInput(Vector.ZERO, new Angle(-speed))));
+        }
     }
-    
+
     @Override
     public void stop() {
-    
+        driveTrain.setInput(new ConstantInput<DTInput>(new DTInput(Vector.ZERO, Angle.ZERO)));
     }
-    
+
     @Override
     public boolean isDone() {
-        return false;
+        int rightTicks = 0;
+        int[] currentRight = driveTrain.getRightEncoderValues();
+        for (int i = 0; i < rightSideStartValues.length; i++) {
+            rightTicks += Math.abs(currentRight[i] - rightSideStartValues[i]);
+        }
+
+        int leftTicks = 0;
+        int[] currentLeft = driveTrain.getLeftEncoderValues();
+        for (int i = 0; i < leftSideStartValues.length; i++) {
+            leftTicks += Math.abs(currentLeft[i] - leftSideStartValues[i]);
+        }
+
+        int ticksMoved = rightTicks + leftTicks;
+
+        return Math.abs(ticksMoved - degrees * ticksPerDegree) < tolerance;
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
index ab17011..1ec9c93 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
@@ -34,17 +34,12 @@ of this software and associated documentation files (the "Software"), to deal
 /**
  * The main class that takes care of setting up the environment, and running all of the main methods
  *
-<<<<<<< HEAD
- * The main purpose behind the alloy class is to control how and when all of the methods in Robot are run
- * as well as initialize all global variables
-=======
  * <p>The alloy class is an extension of the FTC OpMode class, and takes care of preliminary setup
  * of: <br>
  * - Setting up the global FTC variables in RobotCore <br>
  * - Setting up the global Debugger <br>
  * - Initializing any "initializeables" in the project <br>
  * - Running the users defined methods <br>
->>>>>>> c6acf51e58c6fa9fea0d80aa10012c6112d66195
  *
  * @author Garrett Burroughs
  * @version 0.1
@@ -87,6 +82,7 @@ public void init() {
 
         robotSetup();
         initialization();
+        driveTrain.setDefaultInput();
     }
 
     @Override
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Debugger.java b/src/main/java/org/montclairrobotics/alloy/core/Debugger.java
index f227147..31973f3 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Debugger.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Debugger.java
@@ -188,7 +188,6 @@ public void test(String key, Object value) {
         log(key, value, Level.TEST);
     }
 
-
     /**
      * A message that overrides the debug level system
      *
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Encoder.java b/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
index f3bc16d..16e3fe3 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
@@ -61,12 +61,25 @@ public abstract class Encoder extends InputComponent<Integer> {
     /** Max speed that the motor can go Measured in Ticks per Second */
     private double maxSpeed;
 
+    /** Keeps track of the ticks when the encoder was "reset" */
+    private int ticksAtReset;
+
     /**
      * A method that should be overridden by the encoder
      *
      * @return the raw value of encoder ticks that the encoder reads
      */
-    public abstract int getTicks();
+    public abstract int getRawTicks();
+
+    /** Gets the ticks, this value is affected by encoder resets */
+    public int getTicks() {
+        return getRawTicks() - ticksAtReset;
+    }
+
+    /** soft reset the encoder, setting the current value as 0 ticks */
+    public void reset() {
+        ticksAtReset = getRawTicks();
+    }
 
     public Encoder(double distancePerTick, double maxSpeed) {
         this.distancePerTick = distancePerTick;
diff --git a/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java b/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
index 39bd492..dde70ba 100644
--- a/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
+++ b/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
@@ -23,10 +23,11 @@ of this software and associated documentation files (the "Software"), to deal
 */
 package org.montclairrobotics.alloy.drive;
 
-import org.montclairrobotics.alloy.components.InputComponent;
+import java.util.ArrayList;
 import org.montclairrobotics.alloy.motor.Mapper;
 import org.montclairrobotics.alloy.motor.MotorGroup;
 import org.montclairrobotics.alloy.motor.MotorModule;
+import org.montclairrobotics.alloy.utils.Input;
 
 /**
  * Created by MHS Robotics on 12/16/2017.
@@ -35,17 +36,57 @@ of this software and associated documentation files (the "Software"), to deal
  * @since 0.1
  */
 public class DriveTrain extends MotorGroup<DTInput> {
-    public DriveTrain(InputComponent<DTInput> input, Mapper mapper, MotorModule... modules) {
+    Input<DTInput> defaultInput;
+
+    public DriveTrain(Input<DTInput> input, Mapper mapper, MotorModule... modules) {
         super(input, mapper, modules);
+        defaultInput = input;
+    }
+
+    public void setDefaultInput() {
+        setInput(defaultInput);
+    }
+
+    public int[] getEncoderValues() {
+        ArrayList<MotorModule> modules = getModules();
+        int[] values = new int[modules.size()];
+        for (int i = 0; i < values.length; i++) {
+            values[i] = modules.get(i).getEncoder().getTicks();
+        }
+        return values;
     }
 
-    public DriveTrain setInput(InputComponent<DTInput> input) {
-        this.input = input;
-        return this;
+    public int[] getRightEncoderValues() {
+        ArrayList<MotorModule> modules = getModules();
+        int total = 0;
+        for (MotorModule m : modules) {
+            if (m.getOffset().getX() > 0) {
+                total++;
+            }
+        }
+        int[] values = new int[total];
+        for (int i = 0; i < values.length; i++) {
+            if (modules.get(i).getOffset().getX() > 0) {
+                values[i] = modules.get(i).getEncoder().getTicks();
+            }
+        }
+        return values;
     }
 
-    public DriveTrain setMapper(Mapper mapper) {
-        this.mapper = mapper;
-        return this;
+    public int[] getLeftEncoderValues() {
+        ArrayList<MotorModule> modules = getModules();
+        int total = 0;
+        for (MotorModule m : modules) {
+            if (m.getOffset().getX() < 0) {
+                total++;
+            }
+        }
+        int[] values = new int[total];
+        for (int i = 0; i < values.length; i++) {
+            if (modules.get(i).getOffset().getX() < 0) {
+                values[i] = modules.get(i).getEncoder().getTicks();
+            }
+        }
+        return values;
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/drive/TankDrive.java b/src/main/java/org/montclairrobotics/alloy/drive/TankDrive.java
index 350535e..ad42e97 100644
--- a/src/main/java/org/montclairrobotics/alloy/drive/TankDrive.java
+++ b/src/main/java/org/montclairrobotics/alloy/drive/TankDrive.java
@@ -31,7 +31,7 @@ public TankDrive(InputComponent<DTInput> input, MotorModule... modules) {
         super(input, new TankMapper(), modules);
     }
 
-    public TankDrive(MotorModule ... modules){
+    public TankDrive(MotorModule... modules) {
         this(null, modules);
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
index 12dcf39..f76f548 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
@@ -103,7 +103,7 @@ public boolean getInverted() {
     public Encoder getEncoder() {
         return new Encoder() {
             @Override
-            public int getTicks() {
+            public int getRawTicks() {
                 return motor.getCurrentPosition();
             }
         };
diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
index 97149ad..a667d80 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
@@ -208,7 +208,7 @@ public Mode getRunmode() {
     public Encoder getEncoder() {
         return new Encoder() {
             @Override
-            public int getTicks() {
+            public int getRawTicks() {
                 return getPosition();
             }
         };
diff --git a/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java b/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
index 840a5b1..f676b2f 100644
--- a/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
+++ b/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
@@ -26,8 +26,8 @@ of this software and associated documentation files (the "Software"), to deal
 import java.util.ArrayList;
 import java.util.Arrays;
 import org.montclairrobotics.alloy.components.Component;
-import org.montclairrobotics.alloy.components.InputComponent;
 import org.montclairrobotics.alloy.update.Update;
+import org.montclairrobotics.alloy.utils.Input;
 
 /**
  * A motor group is a group of motor modules that run together
@@ -43,15 +43,15 @@ of this software and associated documentation files (the "Software"), to deal
 public class MotorGroup<T> extends Component {
 
     /** The mapper that will be used for power assignment and calculation */
-    public Mapper mapper;
+    private Mapper mapper;
 
     /** The modules contained in the motor group */
-    public ArrayList<MotorModule> modules;
+    private ArrayList<MotorModule> modules;
 
     /** The control input for the motor group */
-    public InputComponent<T> input;
+    private Input<T> input;
 
-    public MotorGroup(InputComponent input, Mapper mapper, MotorModule... modules) {
+    public MotorGroup(Input input, Mapper mapper, MotorModule... modules) {
         this.input = input;
         this.mapper = mapper;
         this.modules = (ArrayList) Arrays.asList(modules);
@@ -63,7 +63,7 @@ public MotorGroup(InputComponent input, Mapper mapper, MotorModule... modules) {
      * @param input
      * @param modules
      */
-    public MotorGroup(InputComponent<Double> input, MotorModule... modules) {
+    public MotorGroup(Input<Double> input, MotorModule... modules) {
         this(input, new DefaultMapper(), modules);
     }
 
@@ -71,4 +71,24 @@ public MotorGroup(InputComponent<Double> input, MotorModule... modules) {
     public void controlPower() {
         mapper.map(input, modules.toArray(new MotorModule[modules.size()]));
     }
+
+    public Mapper getMapper() {
+        return mapper;
+    }
+
+    public void setMapper(Mapper mapper) {
+        this.mapper = mapper;
+    }
+
+    public Input<T> getInput() {
+        return input;
+    }
+
+    public void setInput(Input<T> input) {
+        this.input = input;
+    }
+
+    public ArrayList<MotorModule> getModules() {
+        return modules;
+    }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java b/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
index b3eae92..72567af 100644
--- a/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
+++ b/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
@@ -30,7 +30,9 @@ of this software and associated documentation files (the "Software"), to deal
 import org.montclairrobotics.alloy.components.Step;
 import org.montclairrobotics.alloy.core.Encoder;
 import org.montclairrobotics.alloy.core.Motor;
+import org.montclairrobotics.alloy.exceptions.InvalidConfigurationException;
 import org.montclairrobotics.alloy.update.Update;
+import org.montclairrobotics.alloy.utils.BangBang;
 import org.montclairrobotics.alloy.utils.ConstantInput;
 import org.montclairrobotics.alloy.utils.ErrorCorrection;
 import org.montclairrobotics.alloy.vector.Vector;
@@ -53,24 +55,41 @@ of this software and associated documentation files (the "Software"), to deal
  */
 public class MotorModule extends Component {
     /** The motors that the module will control */
-    public ArrayList<Motor> motors;
+    private ArrayList<Motor> motors;
 
     /** The direction that the modules run for use in a motor group */
-    public Vector direction;
+    private Vector direction;
 
     /** the position of the motor, relative to the center of the motor group */
-    public Vector offset;
+    private Vector offset;
 
     /** An error correction that will control the power */
-    public ErrorCorrection<Double> powerControl;
+    private ErrorCorrection<Double> powerControl;
 
     /** The encoder that keeps track of the position and controls the motors */
-    public Encoder encoder;
+    private Encoder encoder;
 
     /** how fast the module should be running */
-    public double targetPower;
+    private double targetPower;
 
-    public InputComponent<Double> modifier;
+    /**
+     * An input component, that takes in the motor input, and runs it to the output (setting the
+     * motor powers) this way, steps can be added to this modifier, and the input of the module can
+     * be changed
+     */
+    private InputComponent<Double> modifier;
+
+    /** True when the module is being set to a position */
+    private boolean toPotsition = false;
+
+    /** If running to a position, the target position the motor will aim to go to */
+    private double targetPosition;
+
+    /** Error Corrector to get to the right position */
+    private ErrorCorrection<Double> positionCorrection;
+
+    /** How fast the motors move when they are having their position be set */
+    private double movementSpeed;
 
     /**
      * Create a fully functioning motor module
@@ -99,6 +118,10 @@ public MotorModule(
         }
 
         modifier = new InputComponent<Double>() {};
+
+        positionCorrection = new BangBang(1);
+
+        positionCorrection.setInput(encoder);
     }
 
     /**
@@ -134,14 +157,28 @@ public MotorModule setErrorCorrection(ErrorCorrection powerControl) {
         return this;
     }
 
+    public MotorModule setPositionCorrection(ErrorCorrection positionCorrection) {
+        this.positionCorrection = positionCorrection;
+        return this;
+    }
+
     @Update
     public void powerCorrection() {
         if (status.isEnabled()) { // Check if its enabled
-            for (Motor m : motors) {
-                if (powerControl != null) {
-                    m.setMotorPower(targetPower + powerControl.getCorrection());
-                } else {
-                    m.setMotorPower(targetPower);
+            if (!toPotsition) { // Manual operation
+                for (Motor m : motors) {
+                    if (powerControl != null) {
+                        m.setMotorPower(targetPower + powerControl.getCorrection());
+                    } else {
+                        m.setMotorPower(targetPower);
+                    }
+                }
+            } else { // Autonomous operation
+                for (Motor m : motors) {
+                    if (encoder == null)
+                        throw new InvalidConfigurationException(
+                                "Modules must have an encoder to set their position");
+                    m.setMotorPower(positionCorrection.getCorrection() * movementSpeed);
                 }
             }
         } else { // If disabled, set the power to 0
@@ -161,6 +198,16 @@ public void setPower(double power) {
         modifier.applySteps();
         targetPower = modifier.get();
         powerControl.setTarget(modifier.get());
+        toPotsition = false;
+    }
+
+    public void setMovementSpeed(double speed) {
+        this.movementSpeed = speed;
+    }
+
+    public void setTargetPosition(double targetPosition) {
+        positionCorrection.setTarget(targetPosition);
+        toPotsition = true;
     }
 
     public MotorModule setPowerModifier(InputComponent<Double> modifier) {
@@ -181,6 +228,8 @@ public void disableAction() {
         }
     }
 
+    // Autonomous operation
+
     /** @return the motor arraylist in the module */
     public ArrayList<Motor> getMotors() {
         return motors;
@@ -215,4 +264,16 @@ public InputComponent<Double> getModifier() {
     public Vector getOffset() {
         return offset;
     }
+
+    public boolean atTargetPosition(double tolerance) {
+        return Math.abs(encoder.getTicks() - targetPosition) < tolerance;
+    }
+
+    public void toPosition() {
+        toPotsition = true;
+    }
+
+    public void usingPower() {
+        toPotsition = false;
+    }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java b/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
index f0e49a5..1dc7e68 100644
--- a/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
+++ b/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
@@ -32,10 +32,10 @@ public class IntakeMapper implements Mapper<Vector> {
     @Override
     public void map(Vector input, MotorModule... modules) {
         for (MotorModule m : modules) {
-            if (m.offset.getX() > 0) {
-                m.setPower((input.getY() + input.getX()) * Utils.sign(m.direction.getY()));
+            if (m.getOffset().getX() > 0) {
+                m.setPower((input.getY() + input.getX()) * Utils.sign(m.getDirection().getY()));
             } else {
-                m.setPower((input.getY() - input.getX()) * Utils.sign(m.direction.getY()));
+                m.setPower((input.getY() - input.getX()) * Utils.sign(m.getDirection().getY()));
             }
         }
     }
diff --git a/src/main/java/org/montclairrobotics/alloy/test/TestRobot.java b/src/main/java/org/montclairrobotics/alloy/test/TestRobot.java
index bf413ad..e994a9f 100644
--- a/src/main/java/org/montclairrobotics/alloy/test/TestRobot.java
+++ b/src/main/java/org/montclairrobotics/alloy/test/TestRobot.java
@@ -72,7 +72,6 @@ public class TestRobot extends Alloy {
     InputComponent intakeController;
     InputComponent driveStick;
 
-
     @Override
     public void robotSetup() {
 
@@ -82,10 +81,10 @@ public void robotSetup() {
 
         // Create Drivetrain Modules (wheels)
         // By making it a tank drive, we use the default tank drive mapper
-        DriveTrain dt = new TankDrive(
-                new MotorModule(new XY(1, 0), d_rightFront, d_rightBack),
-                new MotorModule(new XY(-1, 0), d_leftFront, d_leftBack)
-        );
+        DriveTrain dt =
+                new TankDrive(
+                        new MotorModule(new XY(1, 0), d_rightFront, d_rightBack),
+                        new MotorModule(new XY(-1, 0), d_leftFront, d_leftBack));
 
         // Create the Drivetrain input
         driveStick = new FTCJoystick(RobotCore.getGamepad1(), FTCJoystick.Side.RIGHT);
@@ -96,7 +95,6 @@ public void robotSetup() {
         // Set the Drivetrain Input
         dt.setInput(driveStick);
 
-
         // =============================================================================================================
         // EXAMPLE INTAKE SETUP
         // =============================================================================================================
@@ -111,16 +109,24 @@ public void robotSetup() {
 
         // Get the encoders from the motors
         Encoder intakeRightEncoder =
-                rightIntakeMotor.getEncoder().setMaxSpeed(100).setDistancePerTick(30); // These 100, and 30, are experimental values
+                rightIntakeMotor
+                        .getEncoder()
+                        .setMaxSpeed(100)
+                        .setDistancePerTick(30); // These 100, and 30, are experimental values
         Encoder intakeLeftEncoder =
                 rightIntakeMotor.getEncoder().setMaxSpeed(100).setDistancePerTick(30);
 
         // Define the right and left modules
-        MotorModule intakeRightSide = new MotorModule(new XY(0, 1), intakeRightEncoder, motorCorrection, rightIntakeMotor);
-        MotorModule intakeLeftSide = new MotorModule(new XY(0, 1), intakeLeftEncoder, motorCorrection, leftIntakeMotor);
+        MotorModule intakeRightSide =
+                new MotorModule(
+                        new XY(0, 1), intakeRightEncoder, motorCorrection, rightIntakeMotor);
+        MotorModule intakeLeftSide =
+                new MotorModule(new XY(0, 1), intakeLeftEncoder, motorCorrection, leftIntakeMotor);
 
         // Create the intake
-        intake = new MotorGroup(intakeController, new IntakeMapper(), intakeRightSide, intakeLeftSide);
+        intake =
+                new MotorGroup(
+                        intakeController, new IntakeMapper(), intakeRightSide, intakeLeftSide);
     }
 
     @Override
diff --git a/src/main/java/org/montclairrobotics/alloy/vector/Polar.java b/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
index 66b3397..a3e3a6b 100644
--- a/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
+++ b/src/main/java/org/montclairrobotics/alloy/vector/Polar.java
@@ -43,7 +43,7 @@ public class Polar implements Vector {
 
     public Polar(double magnitude, Angle angle) {
         this.magnitude = magnitude;
-        this.angle     = angle;
+        this.angle = angle;
     }
 
     /**

From 1c0271a5ee3fd581423d049be358a4ea019c8d06 Mon Sep 17 00:00:00 2001
From: GarrettBurroughs <garrettburroughs02@gmail.com>
Date: Sun, 7 Oct 2018 11:31:26 -0400
Subject: [PATCH 12/16] added autonomous features and samples

---
 README.md                                     |  8 +--
 .../alloy/auto/AlloyAutonomous.java           | 24 +++++++
 .../alloy/auto/SimpleAutonomous.java          |  5 +-
 .../alloy/auto/States/ConditionalState.java   | 64 +++++++++++++++++++
 .../alloy/auto/States/Turn.java               |  4 +-
 .../alloy/test/SquareAuto.java                | 31 +++++++++
 6 files changed, 125 insertions(+), 11 deletions(-)
 create mode 100644 src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
 create mode 100644 src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java

diff --git a/README.md b/README.md
index 2f5e761..10c617b 100644
--- a/README.md
+++ b/README.md
@@ -1,19 +1,14 @@
-<<<<<<< HEAD
-# Alloy  [![Build Status](https://travis-ci.org/GarrettBurroughs/Alloy.svg?branch=master)](https://travis-ci.org/GarrettBurroughs/Alloy) [![CodeFactor](https://www.codefactor.io/repository/github/garrettburroughs/alloy/badge)](https://www.codefactor.io/repository/github/garrettburroughs/alloy) 
-=======
 # Alloy  [![Build Status](https://travis-ci.org/GarrettBurroughs/Alloy.svg?branch=master)](https://travis-ci.org/GarrettBurroughs/Alloy) [![CodeFactor](https://www.codefactor.io/repository/github/garrettburroughs/alloy/badge)](https://www.codefactor.io/repository/github/garrettburroughs/alloy) [![Codacy Badge](https://api.codacy.com/project/badge/Grade/f3335dca15a8429ebc50528ca6330411)](https://www.codacy.com/project/garrett_2/Alloy/dashboard?utm_source=github.com&amp;utm_medium=referral&amp;utm_content=GarrettBurroughs/Alloy&amp;utm_campaign=Badge_Grade_Dashboard)
->>>>>>> c6acf51e58c6fa9fea0d80aa10012c6112d66195
 
 ![AlloyLogo](http://gdurl.com/AFl8)
 
 Status: **Beta**
-<<<<<<< HEAD
 
 
 Alloy is a robot framework designed specifically for the First Tech Challenge (FTC).
 
 Alloy is based off the FRC robot framework [Sprocket](https://github.com/MontclairRobotics/Sprocket), but redesigned to abstract away the more complex ideas but still allow for advanced robot functionality.
-=======
+
 Note: Alloy is still under heavy development, and not ready or suggested for use in creating a robot
 
 Developed by FTC147 and FRC555
@@ -72,4 +67,3 @@ easier debugging, and updateable methods. But again, all of this can be done wit
 
 A. If you have a problem first see our [Troubleshooting guide](https://github.com/GarrettBurroughs/Alloy/wiki/Troubleshooting-Guide). If that does not fix your problem you can file an [Issue](https://github.com/GarrettBurroughs/Alloy/issues/new)
 using our issue template, please provide as much information as possible so we can fully understand the probelm and fix the issue.
->>>>>>> c6acf51e58c6fa9fea0d80aa10012c6112d66195
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index dc852a6..118f4b1 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -25,7 +25,14 @@ of this software and associated documentation files (the "Software"), to deal
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Set;
+
+import org.montclairrobotics.alloy.core.Alloy;
 import org.montclairrobotics.alloy.core.RobotCore;
+import org.reflections.Reflections;
+import org.reflections.scanners.SubTypesScanner;
 
 /**
  * Created by MHS Robotics on 12/5/2017.
@@ -44,6 +51,7 @@ of this software and associated documentation files (the "Software"), to deal
  * @since 0.1
  */
 public abstract class AlloyAutonomous extends OpMode {
+
     /** Keeps track of when the auto mode is running (True after started and before finished) */
     boolean running;
 
@@ -75,6 +83,22 @@ public void start() {
      */
     @Override
     public void init() {
+        Reflections reflections = new Reflections(new SubTypesScanner());
+        Set<Class<? extends Alloy>> robots = reflections.getSubTypesOf(Alloy.class);
+        for (Class<? extends Alloy> robot : robots) {
+            try {
+                if (!Modifier.isAbstract(robot.getMethod("robotSetup").getModifiers())
+                        || robot.getMethod("robotSetup") == null) {
+                    robot.getMethod("robotSetup").invoke(robot.newInstance());
+                }
+            } catch (NoSuchMethodException
+                    | IllegalAccessException
+                    | InstantiationException
+                    | InvocationTargetException e) {
+                e.printStackTrace();
+            }
+        }
+
         new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
         setup();
     }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
index a45f712..a835db1 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
@@ -25,6 +25,7 @@ of this software and associated documentation files (the "Software"), to deal
 
 import org.montclairrobotics.alloy.auto.States.Drive;
 import org.montclairrobotics.alloy.auto.States.Turn;
+import org.montclairrobotics.alloy.vector.Angle;
 
 /**
  * Created by MHS Robotics on 1/26/2018.
@@ -45,7 +46,7 @@ public void drive(double speed, double distance) {
         auto.addState(new Drive(speed, distance));
     }
 
-    public void turn(double speed, double degrees) {
-        auto.addState(new Turn(speed, degrees));
+    public void turn(double speed, Angle angle) {
+        auto.addState(new Turn(speed, angle));
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
new file mode 100644
index 0000000..e3913d6
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
@@ -0,0 +1,64 @@
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.utils.Input;
+
+/**
+ * A conditional state runs a state if the condition is true
+ *
+ * A conditional state takes in a boolean input, and evaluates it when the state starts.
+ * If the input is true when the state starts, the passed in state will execute.
+ */
+public class  ConditionalState extends State {
+    /**
+     * The boolean input that will determine if the state is run
+     */
+    Input<Boolean> condition;
+
+    /**
+     * The state that will be run, if the input is true
+     */
+    State state;
+
+    /**
+     * A variable to keep track whether or not the state should be run
+     */
+    boolean run;
+
+
+    public ConditionalState(Input<Boolean> condition, State state){
+        this.condition = condition;
+        this.state = state;
+    }
+
+    @Override
+    public void start() {
+        run = condition.get(); // Evaluate the condition
+        if(run){
+            state.start();
+        }
+    }
+
+    @Override
+    public void run() {
+        if(run){
+            state.run();
+        }
+    }
+
+    @Override
+    public void stop() {
+        if(run){
+            state.stop();
+        }
+    }
+
+    @Override
+    public boolean isDone() {
+        if (run){
+            return state.isDone();
+        }else{
+            return true;
+        }
+    }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
index fc260af..44c633c 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
@@ -57,9 +57,9 @@ public static void setTolerance(double tolerance) {
         Turn.tolerance = tolerance;
     }
 
-    public Turn(double speed, double degrees) {
+    public Turn(double speed, Angle angle) {
         this.speed = speed;
-        this.degrees = degrees;
+        this.degrees = angle.getDegrees();
         driveTrain = Alloy.getDriveTrain();
     }
 
diff --git a/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
new file mode 100644
index 0000000..43b0e67
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
@@ -0,0 +1,31 @@
+package org.montclairrobotics.alloy.test;
+
+import org.montclairrobotics.alloy.auto.SimpleAutonomous;
+import org.montclairrobotics.alloy.auto.States.Turn;
+import org.montclairrobotics.alloy.vector.Angle;
+
+public class SquareAuto extends SimpleAutonomous {
+
+    @Override
+    public void setup() {
+        Turn.setTicksPerDegree(10); //  This is a dummy value, this value will need to be tested for
+
+        /*
+        Keep in mind, even though states can be added like this, because they are created by methods inherited from
+        Simple autonomous, they are not actually run right here.
+        This means that you can not conditional add states here, for example
+
+        if(sensor.value > 5){
+            drive(1, 5);
+        }
+        a
+        would not work, because it would be reading the sensor value on startup.
+        If you want to run states conditionally, use ConditionalState
+         */
+        for(int i = 0; i < 4; i++){
+            // These functions come from SimpleAutonomous, they just add a new state to the state machine
+            drive(1, 100);
+            turn(1, new Angle(90));
+        }
+    }
+}

From 558b26cba3180090303bd1cae4616cacd48a0644 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Thu, 24 Jan 2019 16:51:08 -0500
Subject: [PATCH 13/16] Fixed Merge conflicts and Removed
 InvalidParameterException

---
 .../Exceptions/InvalidParameterException.java | 32 -------------------
 .../montclairrobotics/alloy/auto/README.md    |  3 --
 2 files changed, 35 deletions(-)
 delete mode 100644 src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java

diff --git a/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java b/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
deleted file mode 100644
index 275c9a5..0000000
--- a/src/main/java/org/montclairrobotics/alloy/Exceptions/InvalidParameterException.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
-MIT License
-
-Copyright (c) 2018 Garrett Burroughs
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-package org.montclairrobotics.alloy.Exceptions;
-
-/**
- * Created by MHS Robotics on 1/26/2018.
- *
- * @author Garrett Burroughs
- * @since
- */
-public class InvalidParameterException extends Exception {}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/README.md b/src/main/java/org/montclairrobotics/alloy/auto/README.md
index da57b95..c6c4e6c 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/README.md
+++ b/src/main/java/org/montclairrobotics/alloy/auto/README.md
@@ -17,9 +17,6 @@ when they have finished running
 3. Alloy Autonomus - This is the basic framework for alloy auto's. An alloy autonomous takes care of running the state machine<br>
 that the auto is using so all the user has to do is create the state machine and define functionality.
 You can read more about how to use an Alloy Autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
-<<<<<<< HEAD
 
 4. Simple Autonomous - A simple autonomous mode further abstracts away auto mode creation allowing 
 for command like auto mode programming. You can read more about how to use a Simple autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
-=======
->>>>>>> c6acf51e58c6fa9fea0d80aa10012c6112d66195

From 37412c624058eb918b6bf77ca47c9b022feb9394 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Thu, 24 Jan 2019 17:00:52 -0500
Subject: [PATCH 14/16] Updated Format and debugging for states

---
 .../alloy/auto/AlloyAutonomous.java           |  1 -
 .../montclairrobotics/alloy/auto/State.java   | 12 +++--
 .../alloy/auto/StateMachine.java              |  5 +-
 .../alloy/auto/States/ConditionalState.java   | 54 ++++++++++++-------
 .../alloy/test/SquareAuto.java                | 25 ++++++++-
 5 files changed, 69 insertions(+), 28 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
index 118f4b1..bacb5e9 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/AlloyAutonomous.java
@@ -28,7 +28,6 @@ of this software and associated documentation files (the "Software"), to deal
 import java.lang.reflect.InvocationTargetException;
 import java.lang.reflect.Modifier;
 import java.util.Set;
-
 import org.montclairrobotics.alloy.core.Alloy;
 import org.montclairrobotics.alloy.core.RobotCore;
 import org.reflections.Reflections;
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/State.java b/src/main/java/org/montclairrobotics/alloy/auto/State.java
index 5035059..d19ef5f 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/State.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/State.java
@@ -37,9 +37,9 @@ of this software and associated documentation files (the "Software"), to deal
  */
 public abstract class State {
 
-    Integer nextState = null;
-    String debug = "Running State: ";
-    String description = "None";
+    private Integer nextState = null;
+    protected String debug = "Running State: ";
+    protected String description = "None";
 
     /** The start method is the first thing called when the state is run */
     public abstract void start();
@@ -80,6 +80,10 @@ public int getNextState(int currentState) {
      * @return debug information about the state
      */
     public String debugInfo(int currentState) {
-        return debug + currentState + "\n Description" + description;
+        return debug + currentState;
+    }
+
+    public String verboseDebug() {
+        return description;
     }
 }
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index 8048121..fd28ca4 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -136,9 +136,8 @@ public void run() {
             }
             stateStarted = false; // The next state has not started yet.
         }
-        Component.debugger.test(
-                name + " | State: " + state,
-                currentState.debugInfo(state)); // Debug info about the state
+        Component.debugger.test(name + " | State: ", state); // Debug info about the state
+        Component.debugger.debug(currentState.debugInfo(state), currentState.verboseDebug());
     }
 
     /** When the state machine is finished, read out it has finished */
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
index e3913d6..d92e0a4 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
@@ -1,3 +1,26 @@
+/*
+MIT License
+
+Copyright (c) 2019 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.auto.States;
 
 import org.montclairrobotics.alloy.auto.State;
@@ -6,27 +29,20 @@
 /**
  * A conditional state runs a state if the condition is true
  *
- * A conditional state takes in a boolean input, and evaluates it when the state starts.
- * If the input is true when the state starts, the passed in state will execute.
+ * <p>A conditional state takes in a boolean input, and evaluates it when the state starts. If the
+ * input is true when the state starts, the passed in state will execute.
  */
-public class  ConditionalState extends State {
-    /**
-     * The boolean input that will determine if the state is run
-     */
+public class ConditionalState extends State {
+    /** The boolean input that will determine if the state is run */
     Input<Boolean> condition;
 
-    /**
-     * The state that will be run, if the input is true
-     */
+    /** The state that will be run, if the input is true */
     State state;
 
-    /**
-     * A variable to keep track whether or not the state should be run
-     */
+    /** A variable to keep track whether or not the state should be run */
     boolean run;
 
-
-    public ConditionalState(Input<Boolean> condition, State state){
+    public ConditionalState(Input<Boolean> condition, State state) {
         this.condition = condition;
         this.state = state;
     }
@@ -34,30 +50,30 @@ public ConditionalState(Input<Boolean> condition, State state){
     @Override
     public void start() {
         run = condition.get(); // Evaluate the condition
-        if(run){
+        if (run) {
             state.start();
         }
     }
 
     @Override
     public void run() {
-        if(run){
+        if (run) {
             state.run();
         }
     }
 
     @Override
     public void stop() {
-        if(run){
+        if (run) {
             state.stop();
         }
     }
 
     @Override
     public boolean isDone() {
-        if (run){
+        if (run) {
             return state.isDone();
-        }else{
+        } else {
             return true;
         }
     }
diff --git a/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
index 43b0e67..728af90 100644
--- a/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
+++ b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
@@ -1,3 +1,26 @@
+/*
+MIT License
+
+Copyright (c) 2019 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
 package org.montclairrobotics.alloy.test;
 
 import org.montclairrobotics.alloy.auto.SimpleAutonomous;
@@ -22,7 +45,7 @@ public void setup() {
         would not work, because it would be reading the sensor value on startup.
         If you want to run states conditionally, use ConditionalState
          */
-        for(int i = 0; i < 4; i++){
+        for (int i = 0; i < 4; i++) {
             // These functions come from SimpleAutonomous, they just add a new state to the state machine
             drive(1, 100);
             turn(1, new Angle(90));

From b62483bd51d7a60c680d4e26dcdb78f619d7dd31 Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Thu, 24 Jan 2019 17:04:27 -0500
Subject: [PATCH 15/16] Updated Description and debug info for state machines

---
 .../alloy/auto/StateMachine.java                 | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index fd28ca4..973d063 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -88,14 +88,26 @@ public StateMachine(String name, int finalState, State... states) {
         this.name = name;
         this.states = new ArrayList<>(Arrays.asList(states));
         this.finalState = finalState;
-        description = "A state machine";
+        description = "";
     }
 
+    public StateMachine(State ... states){
+        this("State Machine", 0, states);
+    }
+    
     public StateMachine(String name, String description, int finalState, State... states) {
         this(name, finalState, states);
         super.description = description;
     }
-
+    
+    public void setFinalState(Integer finalState) {
+        this.finalState = finalState;
+    }
+    
+    public void setName(String name) {
+        this.name = name;
+    }
+    
     /** Read out that the state has started and reset the timer */
     @Override
     public void start() {

From f7efc987f10651bce5df66d061e387b14d2f9d5d Mon Sep 17 00:00:00 2001
From: Garrett Burroughs <garrett@montclairrobotics.org>
Date: Thu, 24 Jan 2019 17:07:55 -0500
Subject: [PATCH 16/16] Updated Simple Autonomous

---
 .../alloy/auto/SimpleAutonomous.java               | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
index a835db1..81e4f4c 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
@@ -23,8 +23,10 @@ of this software and associated documentation files (the "Software"), to deal
 */
 package org.montclairrobotics.alloy.auto;
 
+import org.montclairrobotics.alloy.auto.States.ConditionalState;
 import org.montclairrobotics.alloy.auto.States.Drive;
 import org.montclairrobotics.alloy.auto.States.Turn;
+import org.montclairrobotics.alloy.utils.Input;
 import org.montclairrobotics.alloy.vector.Angle;
 
 /**
@@ -43,10 +45,18 @@ public class SimpleAutonomous extends AlloyAutonomous {
     public void setup() {}
 
     public void drive(double speed, double distance) {
-        auto.addState(new Drive(speed, distance));
+        addState(new Drive(speed, distance));
     }
 
     public void turn(double speed, Angle angle) {
-        auto.addState(new Turn(speed, angle));
+        addState(new Turn(speed, angle));
+    }
+    
+    public void contition(Input<Boolean> condition, State state){
+        addState(new ConditionalState(condition, state));
+    }
+    
+    public void addState(State state){
+        auto.addState(state);
     }
 }