diff --git a/build.gradle b/build.gradle
index 8d64237f..17a2c676 100644
--- a/build.gradle
+++ b/build.gradle
@@ -2,7 +2,7 @@ plugins {
     id "java"
     id 'java-library'
     id 'maven-publish'
-    id "edu.wpi.first.GradleRIO" version "2023.4.1"
+    id "edu.wpi.first.GradleRIO" version "2023.4.2"
     id 'com.google.protobuf' version '0.8.19'
 }
 
diff --git a/docs/FollowPoints.md b/docs/FollowPoints.md
new file mode 100644
index 00000000..0dfb9715
--- /dev/null
+++ b/docs/FollowPoints.md
@@ -0,0 +1,53 @@
+# The FollowPoints() Procedure
+
+## What is FollowPoints?
+
+FollowPoints is a procdure in which the robot follows a path of points. It does not find the robot's position, but utilizes another position-finding system (such as odometry).
+
+## How does FollowPoints work?
+
+FollowPoints utilizes the "Pure Pursuit" method. This method involves a circle drawn around the robot.
+
+![Image](images/followPoints1.png)
+
+At the start of the method, the robot moves towards the first point. In th above image, the robot would move towards Point 1.
+
+![Image](images/followPoints2.png)
+
+Once the circle around the robot intersects the line between Points 1 and 2, the robot starts to move toward the point where this line intersects the circle. In the image above, this is the purple point (the orange point is also a place where the circle and line intersect, but the robot will move towards the intersection point closest to Point 2). The robot will continue moving towards this point, moving closer to the line, until it reaches the line between Points 2 and 3, when it will start to follow that line. This procedure ensures the robot travels along a smooth curve.
+
+## FollowPoints Customization Options
+
+### Critical Points
+
+Though it allows for a smooth curve, Pure Pursuit means that the robot won't actually reach any of the points, and will instead curve around them. It also means that if there are three consecutive and near-colinear points, the method will bypass the second point, as the robot will immediately intersect the line between the second  and third points.
+
+To remedy this, the `FollowPoints()` procedure allows for points to be specified as "critical points." The robot must reach a critical point before moving to the next point. The last point in a `FollowPoints()` procedure is always treated as a critical point, no matter if it is actually specified as one.
+
+### Headers
+
+All points are inputted as `PointDir` objects, which contain a header as an argument. The robot will rotate so it is facing the header of the point it is moving towards.
+
+### Procedures
+
+Optionally, an array of procedures may be provided as an input. If given, once the robot reaches a given point (or starts moving towards the next point in the case the point is not a critical point), it will run that procedure. The boolean `stopRobot` determines whether the robot will stop and wait for the procedure to finish before continuing.
+
+## Desmos Integration
+
+For the 2023 season, FollowPoints has been integrated with [the Desmos model of the field](https://www.desmos.com/calculator/v6mpw9cwbn).
+
+![Image](images/desmosFieldModel.png)
+
+To use this model, drag the points across the field. To make a point a critical point, click the red/gray dots at the bottom of the field (the first dot corresponds to the first point, the second to the second point, etc.). To change the heading at each point, click on the purple dot attached to each point to rotate it by 15 degrees. The model currently does not support procedures.
+
+To add a new point, add its x- and y-coordinate to the table. Add a 0 or 1 to the c_p list corresponding to if the point is critical, and add an angle measure in degrees to the a list for the heading.
+
+To export the path, open the console (Ctrl+Shift+J), scroll to the bottom of the Desmos equations list and copy-and-paste the code into the console. Copy-and-paste the resulting json format into the deploy folder (C:\Users\admin\Documents\GitHub\2023\src\main\deploy).
+
+## Running FollowPoints()
+
+There are several constructors accepted by FollowPoints():
+
+* An array of PointDirs will provide a path, but will not allow for the specification of procedures or critical points.
+* A string with the name of a json file will run the path generated by a Desmos model.
+* A default constructor will run a path with provided "Steps," objects containing a PointDir, a "critical point" boolean, a procedure, and a "stop robot at procedure" boolean.
diff --git a/docs/Odometry.md b/docs/Odometry.md
new file mode 100644
index 00000000..4959ae30
--- /dev/null
+++ b/docs/Odometry.md
@@ -0,0 +1,45 @@
+# Odometry
+
+## What is Odometry?
+
+Odometry is a method to calculate the position of the robot based on how far each of the wheels has traveled.
+
+## How does Odometry work?
+
+On a swerve drive-bot, each wheel can both spin and rotate. We use this to estimate where each robot has traveled. Each wheel keeps track of how far it has traveled, as well as its heading, and we use this to calculate the position of each wheel.
+
+![Image](images/odometry_wheel.png)
+
+Given a wheel's starting position, distance traveled, and angle change, an arc is used to estimate the new x- and y-positions of that wheel. Once positions for all four wheels are calculated, they are averaged to find the new position of the robot.
+
+## Benefits of Odometry
+
+* **It does not require any external hardware.** Odometry is entirely based on the wheels, and does not require any cameras or lights, and so will rarely break. This leaves it as a robust backup in case another position-finding system fails.
+* **It works fast.** Odometry can easily run at 100 or even 1000 times per second, meaning it can be used in the times between updates from another position-finding system.
+
+## Limitations of Odometry
+
+* **It is highly dependent on how accurately the circumference of the wheel can be measured.** Even a small error can lead to odometry thinking the robot is meters away from where it actually is.
+* **Error compounds over time.** Unlike other forms of position-finding, odometry error only gets worse the longer the robot is driven. This puts odometry as best used in-combination with another form of position-finding.
+* **Wheel slippage leads to huge amounts of error.** Odometry assumes there is no wheel slippage, and that if a wheel spins once, it has traveled the length of its circumference. If the robot runs into an object but the wheels keep spinning, the position will be thrown off.
+
+## How to use Odometry
+
+Odometry should be run in the Drive.java mechanism, which allows it to work even when Tele-Op is turned off. 
+
+### Initialization
+
+To initialize Odometry, use the constructor
+`swerveOdometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime)`.
+
+* motorList, CANCoderList, and wheelLocations should contain arrays with an element corresponding to each wheel in the same order.
+* gearRatio and encoderToRevolutionConstant are dependent on the swerve module.
+* rateLimiterTime is how often odometry should run. The faster it runs, the more accurate it will be, but the more memory it takes up.
+
+### Running Odometry
+
+To use odometry, set a PointDir object equal to the `odometry.run()` method within `Robot.drive.run()`.
+
+### Integrating Odometry with other Position-Finding Methods
+
+Use the `setCurrentPosition(Point P)` method to set the odometry current position to a position found from another source.
\ No newline at end of file
diff --git a/docs/Point and PointDir.md b/docs/Point and PointDir.md
new file mode 100644
index 00000000..777afc5c
--- /dev/null
+++ b/docs/Point and PointDir.md	
@@ -0,0 +1,16 @@
+# Point and PointDir
+
+## What are Point and PointDir?
+
+Point and PointDir are two classes which make it easy to represent a position with or without a heading. PointDir inherits from Point.
+
+## How to use Point and PointDir
+
+Points have an x-coordinate and a y-coordinate, which can be accessed via `getX()` and `getY()`. PointDir additionally has a heading, which can be accessed via `getHeading()`.
+
+Point has several useful methods:
+
+* `distance(Point a)` returns the distance between the current point and `a`.
+* `slope(Point a)` returns the slope of the line between the current point and `a`. It has a maximum and minimum of ±1000.
+* `add(Point a)` returns a point with an whose coordinates are the sums of the corresponding coordinates of the current point and `a`.
+* `scaleVector(Point inputPoint, double scale)` returns a point whose coordinates are the coordinates of a vector between the current point and `inputPoint`, scaled to have a length of `scale`.
diff --git a/docs/SwerveDrive.md b/docs/SwerveDrive.md
new file mode 100644
index 00000000..f56501ac
--- /dev/null
+++ b/docs/SwerveDrive.md
@@ -0,0 +1,57 @@
+# Swerve Drive
+
+## What is swerve drive?
+
+Swerve drive is a type of drive train that allows for independent control of each wheel. This allows for the robot to move in any direction, and rotate in place. It also allows for the robot to move sideways or rotate while moving linearly, which is useful for some games.
+
+## How does swerve drive work?
+
+Swerve drive works by having each wheel connected to a motor that can rotate 360 degrees. Each wheel has a steering and a driving motor. The steering motor rotates the wheel, and the driving motor rotates the wheel and the robot.
+
+Here is an example of a swerve drive module (mk4i):
+
+![Image](images/swervedrive.png)
+
+As you can see, there are 2 motors which control the orientation and speed of the wheel. There is also an absolute encoder that allows the robot to know the orientation of the wheel even after it is fully turned off.
+
+When 4 of these modules are connected to a robot, it can move in any direction and rotate in place by using vector addition.
+
+Strafing is the process of moving in 2 dimentions by orienting all of the wheels at the same angle and moving them at the same speed.
+
+Rotating is the process of rotating in place by orienting all of the wheels at 90 degrees angles from each other and then turning them at the same speed.
+
+When strafing and rotating are done at the same time, we get a vector sum of the two vectors for each wheel (speed is a magnitude while orientation is well, orientation). Since rotatin and strafing are going to be different for each wheels, the robot is able to move while rotating.
+
+One caveat is that the added vectors could be greater than 1, there, we need to scale all of the vectors down.
+
+## Benefits of swerve drive
+
+* **Agility.** Because swerve can move in any directions and rotate, it is very easy to avoid defense from other robots. It is also easier to move around the field and score points for most objectives.
+* **Modular.** Because swerve drive modules are modular, the robot is easily repairable It is also possible to drive the robot with only 3 modules working (if one module is broken during a match for example).
+
+## Limitations of sweve drive
+
+* **Very complex.** Because of the complex nature of swerve drive, the code is to read or understand. Moreover, the chance of something going wrong is high.
+* **Draws a lot of power.** Swerve drive uses 8 motors at once, which draw a lot of power, it is **nessessary** to prepare the amount of amps drawn by each motor on the robot.
+
+## How to use swerve drive
+
+Call the `swerveDrive` procedure in the `Drive` mechanism. This procedure takes 3 parameters:
+
+* `x` - The x component of the vector to move in.
+* `y` - The y component of the vector to move in.
+* `rotation` - The amount to rotate the robot.
+
+You can also inpute a pointDir, but this should only be used for `FollowPoints`.
+### Initialization
+
+Calling `Drive()` will initialize the mechanism. This will initialize the motors and the encoders.
+
+### Running swerveDrive
+
+To use the mecanism, call the `swerveDrive` method. You can simply use joysticks as inputs. 
+No procedures are needed for swerve, all is containted in the `Drive.java` mechanism.
+
+### Integrating Mechanism with other code
+
+Thanks to the option to add a pointDir in the method `swerveDrive`, it is possible to use swerve drive with `FollowPoints` (or another procedure).
\ No newline at end of file
diff --git a/docs/candle.md b/docs/candle.md
new file mode 100644
index 00000000..b026f770
--- /dev/null
+++ b/docs/candle.md
@@ -0,0 +1,9 @@
+# A Simple Guide on Using CANdle to Display Single Colors
+
+Use `Robot.candle.setColor(int r, int g, int b);` to display single colors on the CANdle.
+
+**Yellow:** R: 255 G: 255 B: 0
+
+**Purple:** R: 255 G: 0 B: 255
+
+**Off** R: 0 G: 0 B: 0
diff --git a/docs/images/desmosFieldModel.png b/docs/images/desmosFieldModel.png
new file mode 100644
index 00000000..849c504b
Binary files /dev/null and b/docs/images/desmosFieldModel.png differ
diff --git a/docs/images/followPoints1.png b/docs/images/followPoints1.png
new file mode 100644
index 00000000..067267c2
Binary files /dev/null and b/docs/images/followPoints1.png differ
diff --git a/docs/images/followPoints2.png b/docs/images/followPoints2.png
new file mode 100644
index 00000000..b4bfbced
Binary files /dev/null and b/docs/images/followPoints2.png differ
diff --git a/docs/images/odometry_wheel.png b/docs/images/odometry_wheel.png
new file mode 100644
index 00000000..dbfd45d6
Binary files /dev/null and b/docs/images/odometry_wheel.png differ
diff --git a/docs/images/swervedrive.png b/docs/images/swervedrive.png
new file mode 100644
index 00000000..8e01fa67
Binary files /dev/null and b/docs/images/swervedrive.png differ
diff --git a/src/main/deploy/FollowPoints.json b/src/main/deploy/FollowPoints.json
new file mode 100644
index 00000000..9ba1ac18
--- /dev/null
+++ b/src/main/deploy/FollowPoints.json
@@ -0,0 +1,12 @@
+{
+	"points": [
+		{
+			"coordinates": [1, 0, 0],
+			"critical": false,
+			"procedure": {
+				"name" : "DoNothing()",
+				"stop" : false
+			}
+		}
+	]
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/hal/GenericRobotMain.java b/src/main/java/com/team766/hal/GenericRobotMain.java
index 599f9ff3..940fbcce 100755
--- a/src/main/java/com/team766/hal/GenericRobotMain.java
+++ b/src/main/java/com/team766/hal/GenericRobotMain.java
@@ -122,13 +122,13 @@ public void teleopInit() {
 			m_autonMode = null;
 		}
 		
-		if (m_oiContext == null) {
+		if (m_oiContext == null && m_oi != null) {
 			m_oiContext = Scheduler.getInstance().startAsync(m_oi);
 		}
 	}
 
 	public void teleopPeriodic() {
-		if (m_oiContext.isDone()) {
+		if (m_oiContext != null && m_oiContext.isDone()) {
 			m_oiContext = Scheduler.getInstance().startAsync(m_oi);
 			Logger.get(Category.OPERATOR_INTERFACE).logRaw(Severity.WARNING, "Restarting OI context");
 		}
diff --git a/src/main/java/com/team766/hal/RobotProvider.java b/src/main/java/com/team766/hal/RobotProvider.java
index 4698eeb4..6cd2c027 100755
--- a/src/main/java/com/team766/hal/RobotProvider.java
+++ b/src/main/java/com/team766/hal/RobotProvider.java
@@ -24,7 +24,7 @@ public abstract class RobotProvider {
 	public static RobotProvider instance;
 	
 	protected EncoderReader[] encoders = new EncoderReader[20];
-	protected SolenoidController[] solenoids = new SolenoidController[10];
+	protected SolenoidController[] solenoids = new SolenoidController[20];
 	protected GyroReader[] gyros = new GyroReader[13];
 	protected HashMap<String, CameraReader> cams = new HashMap<String, CameraReader>();
 	protected JoystickReader[] joysticks = new JoystickReader[8];
@@ -212,7 +212,7 @@ public DoubleSolenoid getSolenoid(String configName) {
 					.toArray(SolenoidController[]::new));
 			return new DoubleSolenoid(forwardSolenoids, reverseSolenoids);
 		} catch (IllegalArgumentException ex) {
-			Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Solenoid %s not found in config file, using mock solenoid instead", configName);
+			Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Solenoid %s not found in config file, using mock solenoid instead %s", configName, ex.toString());
 			return new DoubleSolenoid(null, null);
 		}
 	}
diff --git a/src/main/java/com/team766/hal/wpilib/Solenoid.java b/src/main/java/com/team766/hal/wpilib/Solenoid.java
index e72f4694..e033aabb 100755
--- a/src/main/java/com/team766/hal/wpilib/Solenoid.java
+++ b/src/main/java/com/team766/hal/wpilib/Solenoid.java
@@ -6,6 +6,6 @@
 
 public class Solenoid extends edu.wpi.first.wpilibj.Solenoid implements SolenoidController {
 	public Solenoid(int channel) {
-		super(PneumaticsModuleType.CTREPCM, channel);
+		super(PneumaticsModuleType.REVPH, channel);
 	}
 }
diff --git a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java
index 70f8ffc8..279c9f28 100755
--- a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java
+++ b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java
@@ -33,6 +33,7 @@
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.PneumaticHub;
 import edu.wpi.first.wpilibj.PneumaticsControlModule;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.SPI;
@@ -94,6 +95,12 @@ public void run() {
 	public WPIRobotProvider() {
 		m_dataRefreshThread = new Thread(m_DataRefreshRunnable, "DataRefreshThread");
 		m_dataRefreshThread.start();
+
+		try {
+			ph.enableCompressorAnalog(80, 100);
+		} catch (Exception ex) {
+			LoggerExceptionUtils.logException(ex);
+		}
 	}
 
 	private MotorController[][] motors =
@@ -102,6 +109,7 @@ public WPIRobotProvider() {
 	// The presence of this object allows the compressor to run before we've declared any solenoids.
 	@SuppressWarnings("unused")
 	private PneumaticsControlModule pcm = new PneumaticsControlModule();
+	private PneumaticHub ph = new PneumaticHub();
 
 	@Override
 	public MotorController getMotor(int index, String configPrefix, MotorController.Type type,
@@ -128,7 +136,7 @@ public MotorController getMotor(int index, String configPrefix, MotorController.
 				motor = new CANVictorMotorController(index);
 				break;
 			case TalonFX:
-				final ValueProvider<String> CANBus = ConfigFileReader.getInstance().getString(type + ".CANBus");
+				final ValueProvider<String> CANBus = ConfigFileReader.getInstance().getString(configPrefix + ".CANBus");
 				motor = new CANTalonFxMotorController(index, CANBus.get());
 				break;
 			case VictorSP:
diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java
new file mode 100644
index 00000000..f056f1a2
--- /dev/null
+++ b/src/main/java/com/team766/odometry/Odometry.java
@@ -0,0 +1,154 @@
+package com.team766.odometry;
+
+import com.team766.framework.LoggingBase;
+import com.team766.hal.MotorController;
+import com.team766.library.RateLimiter;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.team766.logging.Category;
+import com.team766.robot.*;
+
+/**
+ * Method which calculates the position of the robot based on wheel positions.
+ */
+public class Odometry extends LoggingBase {
+
+	private RateLimiter odometryLimiter;
+
+	private MotorController[] motorList;
+	//The order of CANCoders should be the same as in motorList
+	private CANCoder[] CANCoderList;
+	private	int motorCount; 
+
+	private PointDir[] prevPositions;
+	private PointDir[] currPositions;
+	private double[] prevEncoderValues;
+	private double[] currEncoderValues;
+	private double gyroPosition;
+
+	private PointDir currentPosition;
+
+	//In Meters
+	private static double WHEEL_CIRCUMFERENCE;
+	public static double GEAR_RATIO;
+	public static int ENCODER_TO_REVOLUTION_CONSTANT;
+
+	//In the same order as motorList, relative to the center of the robot
+	private Point[] wheelPositions;
+	
+	/**
+	 * Constructor for Odometry, taking in several defines for the robot.
+	 * @param motors A list of every wheel-controlling motor on the robot.
+	 * @param CANCoders A list of the CANCoders corresponding to each wheel, in the same order as motors.
+	 * @param wheelLocations A list of the locations of each wheel, in the same order as motors.
+	 * @param wheelCircumference The circumfrence of the wheels, including treads.
+	 * @param gearRatio The gear ratio of the wheels.
+	 * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels.
+	 * @param rateLimiterTime How often odometry should run.
+	 */
+	public Odometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime) {
+		loggerCategory = Category.ODOMETRY;
+
+		odometryLimiter = new RateLimiter(rateLimiterTime);
+		motorList = motors;
+		CANCoderList = CANCoders;
+		motorCount = motorList.length;
+		log("Motor count " + motorCount);
+		prevPositions = new PointDir[motorCount];
+		currPositions = new PointDir[motorCount];
+		prevEncoderValues = new double[motorCount];
+		currEncoderValues = new double[motorCount];
+
+		wheelPositions = wheelLocations;
+		WHEEL_CIRCUMFERENCE = wheelCircumference;
+		GEAR_RATIO = gearRatio;
+		ENCODER_TO_REVOLUTION_CONSTANT = encoderToRevolutionConstant;
+
+		currentPosition = new PointDir(0, 0, 0);
+		for (int i = 0; i < motorCount; i++) {
+			prevPositions[i] = new PointDir(0,0, 0);
+			currPositions[i] = new PointDir(0,0, 0);
+			prevEncoderValues[i] = 0;
+			currEncoderValues[i] = 0;
+		}
+	}
+
+	public String getName() {
+		return "Odometry";
+	}
+
+	/**
+	 * Sets the current position of the robot to Point P
+	 * @param P The point to set the current robot position to
+	 */
+	public void setCurrentPosition(Point P) {
+		currentPosition.set(P);
+		for (int i = 0; i < motorCount; i++) {
+			prevPositions[i].set(currentPosition.add(wheelPositions[i]));
+			currPositions[i].set(currentPosition.add(wheelPositions[i]));
+		}
+	}
+	
+
+	/**
+	 * Updates the odometry encoder values to the robot encoder values.
+	 */
+	private void setCurrentEncoderValues() {
+		for (int i = 0; i < motorCount; i++) {
+			prevEncoderValues[i] = currEncoderValues[i];
+			currEncoderValues[i] = motorList[i].getSensorPosition();
+		}
+	}
+
+	/**
+	 * Updates the position of each wheel of the robot by assuming each wheel moved in an arc.
+	 */
+	private void updateCurrentPositions() {
+		double angleChange;
+		double radius;
+		double deltaX;
+		double deltaY;
+		gyroPosition = -Robot.gyro.getGyroYaw();
+		Point slopeFactor = new Point(Math.sqrt(Math.cos(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroPitch()) * Math.cos(Robot.gyro.getGyroPitch()) + Math.sin(Robot.gyro.getGyroYaw()) * Math.sin(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroRoll()) * Math.cos(Robot.gyro.getGyroRoll())), Math.sqrt(Math.sin(Robot.gyro.getGyroYaw()) * Math.sin(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroPitch()) * Math.cos(Robot.gyro.getGyroPitch()) + Math.cos(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroYaw()) * Math.cos(Robot.gyro.getGyroRoll()) * Math.cos(Robot.gyro.getGyroRoll())));
+
+		for (int i = 0; i < motorCount; i++) {
+			//prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currentPosition.getY() + 0.5 * DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * Math.sin(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), currPositions[i].getHeading());
+			prevPositions[i].set(currentPosition.add(wheelPositions[i]), currPositions[i].getHeading());
+			currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition);
+			angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading();
+
+			if (angleChange != 0) {
+				radius = 180 * (currEncoderValues[i] - prevEncoderValues[i]) / (Math.PI * angleChange);
+				deltaX = radius * Math.sin(Math.toRadians(angleChange));
+				deltaY = radius * (1 - Math.cos(Math.toRadians(angleChange)));
+				currPositions[i].setX(prevPositions[i].getX() + (Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaX - Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
+				currPositions[i].setY(prevPositions[i].getY() + (Math.sin(Math.toRadians(prevPositions[i].getHeading())) * deltaX + Math.cos(Math.toRadians(prevPositions[i].getHeading())) * deltaY) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
+			} else {
+				currPositions[i].setX(prevPositions[i].getX() + (currEncoderValues[i] - prevEncoderValues[i]) * Math.cos(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getX() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
+				currPositions[i].setY(prevPositions[i].getY() + (currEncoderValues[i] - prevEncoderValues[i]) * Math.sin(Math.toRadians(prevPositions[i].getHeading())) * slopeFactor.getY() * WHEEL_CIRCUMFERENCE / (GEAR_RATIO * ENCODER_TO_REVOLUTION_CONSTANT));
+			}
+		}
+	}
+
+	/**
+	 * Calculates the position of the robot by finding the average of the wheel positions.
+	 */
+	private void findRobotPosition() {
+		double sumX = 0;
+		double sumY = 0;
+		for (int i = 0; i < motorCount; i++) {
+			sumX += currPositions[i].getX();
+			sumY += currPositions[i].getY();
+		}
+		currentPosition.set(sumX / motorCount, sumY / motorCount, gyroPosition);
+	}
+
+	//Intended to be placed inside Robot.drive.run()
+	public PointDir run() {
+		if (odometryLimiter.next()) {
+			setCurrentEncoderValues();
+			updateCurrentPositions();
+			findRobotPosition();
+		}
+		return currentPosition;
+	}
+}
diff --git a/src/main/java/com/team766/odometry/Point.java b/src/main/java/com/team766/odometry/Point.java
new file mode 100644
index 00000000..ac6e4a89
--- /dev/null
+++ b/src/main/java/com/team766/odometry/Point.java
@@ -0,0 +1,84 @@
+package com.team766.odometry;
+
+import com.team766.framework.LoggingBase;
+import java.lang.Math;
+
+/**
+ * Class of two-coordinate objects, an x-coordinate and a y-coordinate.
+ */
+public class Point extends LoggingBase {
+	private double x;
+	private double y;
+
+	public String getName() {
+		return "Points";
+	}
+
+	public Point(double x, double y){
+		this.x = x;
+		this.y = y;
+	}
+
+	public double getX() {
+		return x;
+	}
+
+	public double getY() {
+		return y;
+	}
+
+	public void set(double x, double y) {
+		this.x = x;
+		this.y = y;
+	}
+
+	public void set(Point P) {
+		this.x = P.getX();
+		this.y = P.getY();
+	}
+
+	public void setX(double x) {
+		this.x = x;
+	}
+
+	public void setY(double y) {
+		this.y = y;
+	}
+
+	public double distance(Point a) {
+		return Math.sqrt(Math.pow(a.getX() - getX(), 2.0) + Math.pow(a.getY() - getY(), 2.0));
+	}
+
+	public double slope(Point a) {
+		double s;
+		final int MAX = 1000; 
+		if (a.getX() == getX()) {
+			//If the points are on top of each other, returns positive or negative MAX.
+			if (a.getY() < getY()) {
+				s = -MAX;
+			} else {
+				s = MAX;
+			}
+		} else {
+			s = (getY() - a.getY()) / (getX() - a.getX());
+		}
+		if (Math.abs(s) > MAX) {
+			s = Math.signum(s) * MAX;
+		}
+		return s;
+	}
+
+	//Gets a vector with length scale between the point and another point. Used for swerve drive method input.
+	public Point scaleVector(Point inputPoint, double scale) {
+		double d = distance(inputPoint);
+		return new Point((inputPoint.getX() - getX()) * scale / d, (inputPoint.getY() - getY()) * scale / d);
+	}
+
+	public Point add(Point p) {
+		return new Point(getX() + p.getX(), getY() + p.getY());
+	}
+
+	public String toString() {
+		return "X: " + getX() + " Y: " + getY();
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/odometry/PointDir.java b/src/main/java/com/team766/odometry/PointDir.java
new file mode 100644
index 00000000..2f00c413
--- /dev/null
+++ b/src/main/java/com/team766/odometry/PointDir.java
@@ -0,0 +1,82 @@
+package com.team766.odometry;
+
+import com.team766.logging.Category;
+import java.lang.Math;
+
+public class PointDir extends Point {
+	private double heading;
+
+	public PointDir(double x, double y, double h){
+		super(x, y);
+		heading = h;
+		loggerCategory = Category.DRIVE;
+	}
+
+	public PointDir(double x, double y){
+		super(x, y);
+		loggerCategory = Category.DRIVE;
+	}
+
+	public PointDir(Point P){
+		super(P.getX(), P.getY());
+		loggerCategory = Category.DRIVE;
+	}
+
+	public PointDir(Point P, double h) {
+		super(P.getX(), P.getY());
+		heading = h;
+		loggerCategory = Category.DRIVE;
+	}
+
+	public double getHeading() {
+		return heading;
+	}
+
+	public void set(double x, double y, double h) {
+		super.set(x, y);
+		heading = h;
+	}
+
+	public void set(Point P, double h) {
+		super.set(P);
+		heading = h;
+	}
+
+	public void setHeading(double h) {
+		heading = h;
+	}
+
+	public double getAngleDifference(Point a) {
+		//Returns a number between -1 and 1 to represent the number of rotations between the two angles.
+		PointDir unitVector;
+		if (distance(a) == 0) {
+			unitVector = new PointDir(1, 0, 0);
+		} else {
+			unitVector = new PointDir((a.getX() - getX()) / distance(a), (a.getY() - getY()) / distance(a), Math.toDegrees(Math.atan2((a.getY() - getY()) / distance(a), (a.getX() - getX()) / distance(a))));
+		}
+		double headingAngle = getHeading() % 360;
+		if (headingAngle < 0) {
+			headingAngle += 360;
+		}
+		if (unitVector.getHeading() < 0) {
+			unitVector.setHeading(unitVector.getHeading() + 360);
+		}
+		double diff = headingAngle - unitVector.getHeading();
+		if (diff < -180) {
+			diff += 360;
+		} else if (diff > 180) {
+			diff -= 360;
+		}
+		return diff / 180;
+	}
+
+	@Override
+	public String toString() {
+		return super.toString() + " Heading: " + getHeading();
+	}
+
+	@Override
+	public PointDir clone() {
+		return new PointDir(getX(), getY(), getHeading());
+	}
+}
diff --git a/src/main/java/com/team766/robot/AutonomousModes.java b/src/main/java/com/team766/robot/AutonomousModes.java
index 0528d3ac..fb038b87 100644
--- a/src/main/java/com/team766/robot/AutonomousModes.java
+++ b/src/main/java/com/team766/robot/AutonomousModes.java
@@ -2,8 +2,12 @@
 
 import com.team766.framework.AutonomousMode;
 import com.team766.robot.procedures.*;
+import com.team766.odometry.Point;
+import com.team766.odometry.PointDir;
+import com.team766.framework.Procedure;
 
 public class AutonomousModes {
+
 	public static final AutonomousMode[] AUTONOMOUS_MODES = new AutonomousMode[] {
 		// Add autonomous modes here like this:
 		//    new AutonomousMode("NameOfAutonomousMode", () -> new MyAutonomousProcedure()),
@@ -12,7 +16,9 @@ public class AutonomousModes {
 		// define one or more different autonomous modes with it like this:
 		//    new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)),
 		//    new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
-
+		//new AutonomousMode("FollowPoints", () -> new FollowPoints()),
+		new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")),
+		//new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})),
 		new AutonomousMode("DoNothing", () -> new DoNothing()),
 	};
 }
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java
index b33f97e4..e9e7b34f 100644
--- a/src/main/java/com/team766/robot/OI.java
+++ b/src/main/java/com/team766/robot/OI.java
@@ -1,12 +1,17 @@
 package com.team766.robot;
 
 import com.team766.framework.Procedure;
+
+import java.io.IOException;
+
 import com.team766.framework.Context;
 import com.team766.hal.JoystickReader;
 import com.team766.hal.RobotProvider;
 import com.team766.logging.Category;
+import com.team766.robot.constants.InputConstants;
 import com.team766.robot.procedures.*;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -16,23 +21,182 @@ public class OI extends Procedure {
 	private JoystickReader joystick0;
 	private JoystickReader joystick1;
 	private JoystickReader joystick2;
+	private double RightJoystick_X = 0;
+	private double RightJoystick_Y = 0;
+	private double RightJoystick_Z = 0;
+	private double RightJoystick_Theta = 0;
+	private double LeftJoystick_X = 0;
+	private double LeftJoystick_Y = 0;
+	private double LeftJoystick_Z = 0;
+	private double LeftJoystick_Theta = 0;
+	private boolean isCross = false;
+	double turningValue = 0;
 	
+	enum IntakeState {
+		IDLE,
+		SPINNINGOUT,
+		SPINNINGIN
+	}
+	IntakeState intakeState = IntakeState.IDLE;
+
 	public OI() {
 		loggerCategory = Category.OPERATOR_INTERFACE;
 
 		joystick0 = RobotProvider.instance.getJoystick(0);
 		joystick1 = RobotProvider.instance.getJoystick(1);
 		joystick2 = RobotProvider.instance.getJoystick(2);
+
 	}
 	
 	public void run(Context context) {
+		context.takeOwnership(Robot.drive);
+		context.takeOwnership(Robot.intake);
+		context.takeOwnership(Robot.arms);
+		context.takeOwnership(Robot.grabber);
+		context.takeOwnership(Robot.storage);
+		context.takeOwnership(Robot.gyro);
+		
 		while (true) {
 			// wait for driver station data (and refresh it using the WPILib APIs)
 			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
 			RobotProvider.instance.refreshDriverStationData();
+			LeftJoystick_X = Robot.drive.correctedJoysticks(joystick0.getAxis(0));
+			LeftJoystick_Y = Robot.drive.correctedJoysticks(joystick0.getAxis(1));
+			RightJoystick_X = Robot.drive.correctedJoysticks(joystick1.getAxis(0));;
 
+			
 			// Add driver controls here - make sure to take/release ownership
 			// of mechanisms when appropriate.
+			/* if (joystick0.getButtonPressed(15)){
+				if (intakeState == IntakeState.IDLE){
+					Robot.intake.intakeIn();
+					Robot.storage.beltIn();
+					intakeState = IntakeState.SPINNINGIN;
+				} else {
+					Robot.intake.intakeIdle();
+					Robot.storage.beltIdle();
+					intakeState = IntakeState.IDLE;
+				}
+			}
+
+			if (joystick0.getButtonPressed(16)){
+				if (intakeState == IntakeState.IDLE){
+					Robot.intake.intakeOut();
+					Robot.storage.beltOut();
+					intakeState = IntakeState.SPINNINGOUT;
+				} else {
+					Robot.intake.intakeIdle();
+					Robot.storage.beltIdle();
+					intakeState = IntakeState.IDLE;
+				}
+			} */
+
+			if(joystick1.getButton(2)){
+				Robot.drive.setGyro(0);
+			}else{
+				Robot.drive.setGyro(Robot.gyro.getGyroYaw());
+			}		
+
+			if(Math.abs(joystick1.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05){
+				RightJoystick_Y = joystick1.getAxis(InputConstants.AXIS_FORWARD_BACKWARD);
+			} else {
+				RightJoystick_Y = 0;
+			}
+			if(Math.abs(joystick1.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05){
+				RightJoystick_X = joystick1.getAxis(InputConstants.AXIS_LEFT_RIGHT)/2;
+				if(joystick1.getButton(3)){
+					RightJoystick_X *= 2;
+				}	
+			} else {
+				RightJoystick_X = 0;	
+			}
+			if(Math.abs(joystick1.getAxis(InputConstants.AXIS_TWIST)) > 0.05){
+				RightJoystick_Theta = joystick1.getAxis(InputConstants.AXIS_TWIST);
+			} else {
+				RightJoystick_Theta = 0;
+			}
+			if(Math.abs(joystick0.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) > 0.05){
+				LeftJoystick_Y = joystick0.getAxis(InputConstants.AXIS_FORWARD_BACKWARD);
+			} else {
+				LeftJoystick_Y = 0;
+			}
+			if(Math.abs(joystick0.getAxis(InputConstants.AXIS_LEFT_RIGHT)) > 0.05){
+				LeftJoystick_X = joystick0.getAxis(InputConstants.AXIS_LEFT_RIGHT);
+			} else {
+				LeftJoystick_X = 0;
+			}
+			if(Math.abs(joystick0.getAxis(InputConstants.AXIS_TWIST)) > 0.05){
+				LeftJoystick_Theta = joystick0.getAxis(InputConstants.AXIS_TWIST);
+			} else {
+				LeftJoystick_Theta = 0;
+			}
+			//log(Robot.gyro.getGyroYaw());			
+			//TODO: fix defense: the robot basically locks up if there is defense
+			// if(joystick0.getButton(InputConstants.CROSS_DEFENSE)){
+			// 	context.startAsync(new DefenseCross());
+			// }
+			
+			/*if(Math.pow(Math.pow(joystick0.getAxis(InputConstants.AXIS_LEFT_RIGHT),2) + Math.pow(joystick0.getAxis(InputConstants.AXIS_FORWARD_BACKWARD),2), 0.5) > 0.15 ){
+				Robot.drive.drive2D(
+					((joystick0.getAxis(InputConstants.AXIS_LEFT_RIGHT))),
+					((joystick0.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)))
+				);
+			}  else {
+				if(Math.abs(joystick0.getAxis(InputConstants.AXIS_TWIST))>=0.1){
+					Robot.drive.turning(joystick0.getAxis(InputConstants.AXIS_TWIST));
+				} else {
+				Robot.drive.stopDriveMotors();
+				Robot.drive.stopSteerMotors();
+				}
+			}*/
+			if(joystick0.getButtonPressed(1))
+				Robot.gyro.resetGyro();
+
+			if(joystick0.getButtonPressed(11))
+				Robot.drive.resetCurrentPosition();
+
+			if(joystick1.getButtonPressed(1))
+				isCross = !isCross;
+			
+			
+			if(joystick0.getButtonPressed(2)){
+				Robot.drive.setFrontRightEncoders();
+				Robot.drive.setFrontLeftEncoders();
+				Robot.drive.setBackRightEncoders();
+				Robot.drive.setBackLeftEncoders();
+			}
+			// if(joystick1.getButton(1)){
+			// 	turningValue = joystick1.getAxis(InputConstants.AXIS_LEFT_RIGHT); 
+			// } else {
+			// 	turningValue = 0;
+			// }
+
+			SmartDashboard.putNumber("Front left", Robot.drive.getFrontLeft());
+			SmartDashboard.putNumber("Front right", Robot.drive.getFrontRight());
+			SmartDashboard.putNumber("Back left", Robot.drive.getBackLeft());
+			SmartDashboard.putNumber("Back right", Robot.drive.getBackRight());
+
+			if (isCross)  {
+				context.startAsync(new setCross());
+			/*} else if (joystick0.getButton(3)) {
+				Robot.drive.swerveDrive(0, 0.2, 0);*/
+			} else if(Math.abs(LeftJoystick_X)+
+			Math.abs(LeftJoystick_Y) +  Math.abs(RightJoystick_X) > 0) {
+				Robot.drive.swerveDrive( 
+					(LeftJoystick_X),
+			 		(-LeftJoystick_Y),
+			 		(RightJoystick_X));
+			} else {
+				Robot.drive.stopDriveMotors();
+				Robot.drive.stopSteerMotors();				
+			} 
+			
+		
+			
+			if(joystick0.getButtonPressed(1))
+				Robot.gyro.resetGyro();
+
+
 		}
 	}
 }
diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java
index b2b3c2a7..11c96c48 100644
--- a/src/main/java/com/team766/robot/Robot.java
+++ b/src/main/java/com/team766/robot/Robot.java
@@ -4,10 +4,23 @@
 
 public class Robot {
 	// Declare mechanisms here
-	
+	public static Drive drive;
+	public static Gyro gyro;
+	public static CANdleMech candle;
+	public static Intake intake;
+	public static Storage storage;
+	public static Arms arms;
+	public static Grabber grabber;
 
 	public static void robotInit() {
 		// Initialize mechanisms here
-		
+		drive = new Drive();
+		gyro = new Gyro();
+		candle = new CANdleMech();
+		intake = new Intake();
+		storage = new Storage();
+		arms = new Arms();
+		grabber = new Grabber();
+
 	}
 }
diff --git a/src/main/java/com/team766/robot/constants/Config.txt b/src/main/java/com/team766/robot/constants/Config.txt
new file mode 100644
index 00000000..4eb7c3f9
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/Config.txt
@@ -0,0 +1,3 @@
+{
+	
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/constants/ExampleInputConstants.java b/src/main/java/com/team766/robot/constants/ExampleInputConstants.java
new file mode 100644
index 00000000..aa7cb28b
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/ExampleInputConstants.java
@@ -0,0 +1,40 @@
+package com.team766.robot.constants;
+
+/**
+ * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc.
+ * 
+ */
+public final class ExampleInputConstants {
+
+	// "joysticks"
+	public static final int LEFT_JOYSTICK = 0;
+	public static final int RIGHT_JOYSTICK = 1;
+	public static final int CONTROL_PANEL = 2;
+
+	// navigation
+	public static final int AXIS_LEFT_RIGHT = 0;
+	public static final int AXIS_FORWARD_BACKWARD = 1;
+
+
+	public static final int AXIS_SHOOTER_DIAL = 3;
+
+	public static final int CONTROL_PANEL_SHOOTER_SWITCH = 1;
+	public static final int CONTROL_PANEL_INTAKE_BUTTON = 2;
+
+	public static final int CONTROL_PANEL_BELTS_BUTTON = 3;
+
+	public static final int CONTROL_PANEL_SPITBALL_BUTTON = 14;
+
+	public static final int CONTROL_PANEL_AUTOCLIMB_BUTTON = 9;
+	public static final int CONTROL_PANEL_ARMS_SWITCH = 13;
+	public static final int CONTROL_PANEL_ELEVATOR_UP_BUTTON = 7;
+	public static final int CONTROL_PANEL_ELEVATOR_DOWN_BUTTON = 8;
+	public static final int CONTROL_PANEL_ELEVATOR_TOP_BUTTON = 5;
+	public static final int CONTROL_PANEL_ELEVATOR_BOTTOM_BUTTON = 6;
+	public static final int CONTROL_PANEL_AUTO_SHOOT = 10;
+	public static final int JOYSTICK_TRIGGER = 1;
+	public static final int E_STOP = 11;
+	public static final int JOYSTICK_ELEVATOR_RESET_BUTTON = 2;
+	public static final int JOYSTICK_CLIMB_RUNG_BUTTON = 4;
+	public static final int JOYSTICK_CLIMB_FIRST_RUNG_BUTTON = 5;
+}
diff --git a/src/main/java/com/team766/robot/constants/FollowPointsInputConstants.java b/src/main/java/com/team766/robot/constants/FollowPointsInputConstants.java
new file mode 100644
index 00000000..31fa5068
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/FollowPointsInputConstants.java
@@ -0,0 +1,12 @@
+package com.team766.robot.constants;
+
+public final class FollowPointsInputConstants {
+	
+	public static double RADIUS = 0.5;
+	public static double SPEED = 0.5;
+	public static double RATE_LIMITER_TIME = 0.01;
+	public static double MAX_ROTATION_SPEED = 0.2;
+	public static double ANGLE_DISTANCE_FOR_MAX_SPEED = 90;
+
+
+}
diff --git a/src/main/java/com/team766/robot/constants/InputConstants.java b/src/main/java/com/team766/robot/constants/InputConstants.java
new file mode 100644
index 00000000..97970e25
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/InputConstants.java
@@ -0,0 +1,24 @@
+package com.team766.robot.constants;
+
+/**
+ * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc.
+ * 
+ * TODO: consider moving this into a config file.
+ */
+public final class InputConstants {
+
+	//Joysticks
+	public static final int LEFT_JOYSTICK = 0;
+	public static final int RIGHT_JOYSTICK = 1;
+	public static final int CONTROL_PANEL = 2;
+
+	//Navigation
+	public static final int AXIS_LEFT_RIGHT = 0;
+	public static final int AXIS_FORWARD_BACKWARD = 1;
+	public static final int AXIS_TWIST = 3;
+	// Joystick buttons
+	public static final int CROSS_DEFENSE = 1;
+	public static final int RESET_GYRO = 1;
+	public static final int RESET_CURRENT_POSITION = 11;
+	//Other buttons
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/constants/OdometryInputConstants.java b/src/main/java/com/team766/robot/constants/OdometryInputConstants.java
new file mode 100644
index 00000000..ac3be89a
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/OdometryInputConstants.java
@@ -0,0 +1,17 @@
+package com.team766.robot.constants;
+
+import com.team766.hal.MotorController;
+
+/**
+ * Constants used for the Operator Interface, eg for joyticks, buttons, dials, etc.
+ * 
+ */
+public final class OdometryInputConstants {
+
+	public static double WHEEL_CIRCUMFERENCE = 30.5 / 100;
+	public static double GEAR_RATIO = 6.75;
+	public static int ENCODER_TO_REVOLUTION_CONSTANT = 2048;
+	public static double DISTANCE_BETWEEN_WHEELS = 24 * 2.54 / 100;
+	public static double RATE_LIMITER_TIME = 0.05;
+
+}
diff --git a/src/main/java/com/team766/robot/constants/SwerveDriveConstants.java b/src/main/java/com/team766/robot/constants/SwerveDriveConstants.java
new file mode 100644
index 00000000..5699ee58
--- /dev/null
+++ b/src/main/java/com/team766/robot/constants/SwerveDriveConstants.java
@@ -0,0 +1,28 @@
+package com.team766.robot.constants;
+
+public final class SwerveDriveConstants {
+	
+	public static double FirstFrontRightAngle = 135;
+	public static double FirstFrontLeftAngle = 45;
+	public static double FirstBackRightAngle = -135;
+	public static double FirstBackLeftAngle = -45;
+
+	public static double SecondFrontRightAngle = -45;
+	public static double SecondFrontLeftAngle = -135;
+	public static double SecondBackRightAngle = 45;
+	public static double SecondBackLeftAngle = 135;
+
+	public static double ratio = 2048.0 / 360.0 * (150.0 / 7.0);
+
+	public static double P = 0.2;
+	public static double I = 0.0;
+	public static double D = 0.1;
+	public static double FF = 0.0;
+
+	public static double CrossFrontRightAngle = -45;
+	public static double CrossFrontLeftAngle = 45;
+	public static double CrossBackRightAngle = 135;
+	public static double CrossBackLeftAngle = -135;
+
+	
+}
diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java
new file mode 100644
index 00000000..28d07cc2
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Arms.java
@@ -0,0 +1,16 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+
+public class Arms extends Mechanism {
+
+	private MotorController firstJoint;
+	private MotorController secondJoint;
+
+	public Arms(){
+		firstJoint = RobotProvider.instance.getMotor("arms.firstJoint");
+		secondJoint = RobotProvider.instance.getMotor("arms.secondJoint");
+	}
+}
diff --git a/src/main/java/com/team766/robot/mechanisms/CANdleMech.java b/src/main/java/com/team766/robot/mechanisms/CANdleMech.java
new file mode 100644
index 00000000..1b8f6788
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/CANdleMech.java
@@ -0,0 +1,50 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.RobotProvider;
+import com.team766.logging.Severity;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+import com.team766.hal.MotorController;
+
+import java.io.*;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.*;
+
+import javax.imageio.ImageIO;
+
+import com.ctre.phoenix.led.*;
+import com.ctre.phoenix.led.CANdle.LEDStripType;
+import com.ctre.phoenix.led.CANdle.VBatOutputMode;
+import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
+import com.ctre.phoenix.led.LarsonAnimation.BounceMode;
+import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent;
+import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.awt.image.BufferedImage;
+import java.awt.image.Raster;
+
+public class CANdleMech extends Mechanism {
+
+	private final CANdle m_candle = new CANdle(5);
+
+	public CANdleMech() {
+		
+	}
+
+	public void setColor(short r, short g, short b, int index, int count) {
+		checkContextOwnership();
+		m_candle.setLEDs(r / 5, g / 5, b / 5, 0, index, count);
+	}
+
+	public void setColor(int r, int g, int b) {
+		checkContextOwnership();
+		m_candle.setLEDs(r / 5, g / 5, b / 5);
+	}
+	
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java
new file mode 100644
index 00000000..bd92cb8b
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Drive.java
@@ -0,0 +1,664 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.EncoderReader;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.MotorController;
+import com.team766.hal.MotorController.ControlMode;
+import com.team766.hal.simulator.Encoder;
+import com.team766.hal.MotorController;
+import com.team766.library.RateLimiter;
+import com.team766.library.ValueProvider;
+import com.team766.logging.Category;
+import com.team766.simulator.ProgramInterface.RobotMode;
+import com.team766.config.ConfigFileReader;
+import com.team766.framework.Mechanism;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.sensors.AbsoluteSensorRange;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+import com.team766.odometry.Odometry;
+import com.team766.odometry.Point;
+import com.team766.odometry.PointDir;
+import com.team766.hal.MotorControllerCommandFailedException;
+import com.team766.robot.constants.*;
+
+public class Drive extends Mechanism {
+
+	private MotorController m_DriveFrontRight;
+	private MotorController m_DriveFrontLeft;
+	private MotorController m_DriveBackRight;
+	private MotorController m_DriveBackLeft;
+
+	private MotorController m_SteerFrontRight;
+	private MotorController m_SteerFrontLeft;
+	private MotorController m_SteerBackRight;
+	private MotorController m_SteerBackLeft;
+
+	private CANCoder e_FrontRight;
+	private CANCoder e_FrontLeft;
+	private CANCoder e_BackRight;
+	private CANCoder e_BackLeft;
+
+	private ValueProvider<Double> drivePower;
+
+	private double gyroValue;
+
+	private static PointDir currentPosition;
+
+	private MotorController[] motorList;
+	private CANCoder[] CANCoderList;
+	private Point[] wheelPositions;
+	private Odometry swerveOdometry;
+
+	public Drive() {
+
+		loggerCategory = Category.DRIVE;
+		// Initializations of motors
+		// Initialize the drive motors
+		m_DriveFrontRight = RobotProvider.instance.getMotor("drive.DriveFrontRight");
+		m_DriveFrontLeft = RobotProvider.instance.getMotor("drive.DriveFrontLeft");
+		m_DriveBackRight = RobotProvider.instance.getMotor("drive.DriveBackRight");
+		m_DriveBackLeft = RobotProvider.instance.getMotor("drive.DriveBackLeft");
+		// Initialize the steering motors
+		m_SteerFrontRight = RobotProvider.instance.getMotor("drive.SteerFrontRight");
+		m_SteerFrontLeft = RobotProvider.instance.getMotor("drive.SteerFrontLeft");
+		m_SteerBackRight = RobotProvider.instance.getMotor("drive.SteerBackRight");
+		m_SteerBackLeft = RobotProvider.instance.getMotor("drive.SteerBackLeft");
+
+		// Setting up the "config"
+		CANCoderConfiguration config = new CANCoderConfiguration();
+		config.absoluteSensorRange = AbsoluteSensorRange.Signed_PlusMinus180;
+		// The encoders output "encoder" values, so we need to convert that to degrees (because that
+		// is what the cool kids are using)
+		config.sensorCoefficient = 360.0 / 4096.0;
+		// The offset is going to be changed in ctre, but we can change it here too.
+		// config.magnetOffsetDegrees = Math.toDegrees(configuration.getOffset());
+		config.sensorDirection = true;
+
+		// initialize the encoders
+		e_FrontRight = new CANCoder(22, "Swervavore");
+		// e_FrontRight.configAllSettings(config, 250);
+		e_FrontLeft = new CANCoder(23, "Swervavore");
+		// e_FrontLeft.configAllSettings(config, 250);
+		e_BackRight = new CANCoder(21, "Swervavore");
+		// e_BackRight.configAllSettings(config, 250);
+		e_BackLeft = new CANCoder(24, "Swervavore");
+		// e_BackLeft.configAllSettings(config, 250);
+
+
+		// Current limit for motors to avoid breaker problems (mostly to avoid getting electrical
+		// people to yell at us)
+		m_DriveFrontRight.setCurrentLimit(35);
+		m_DriveFrontLeft.setCurrentLimit(35);
+		m_DriveBackRight.setCurrentLimit(35);
+		m_DriveBackLeft.setCurrentLimit(35);
+		m_DriveBackLeft.setInverted(true);
+		m_DriveBackRight.setInverted(true);
+		m_SteerFrontRight.setCurrentLimit(30);
+		m_SteerFrontLeft.setCurrentLimit(30);
+		m_SteerBackRight.setCurrentLimit(30);
+		m_SteerBackLeft.setCurrentLimit(30);
+
+		// Setting up the connection between steering motors and cancoders
+		// m_SteerFrontRight.setRemoteFeedbackSensor(e_FrontRight, 0);
+		// m_SteerFrontLeft.setRemoteFeedbackSensor(e_FrontLeft, 0);
+		// m_SteerBackRight.setRemoteFeedbackSensor(e_BackRight, 0);
+		// m_SteerBackLeft.setRemoteFeedbackSensor(e_BackLeft, 0);
+
+		m_SteerFrontRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
+		m_SteerFrontLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
+		m_SteerBackRight.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
+		m_SteerBackLeft.setSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
+		configPID();
+
+		// Sets up odometry
+		currentPosition = new PointDir(0, 0, 0);
+		motorList = new MotorController[] {m_DriveFrontRight, m_DriveFrontLeft, m_DriveBackLeft,
+				m_DriveBackRight};
+		CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight};
+		wheelPositions =
+				new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2),
+						new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2),
+						new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2),
+						new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)};
+		log("MotorList Length: " + motorList.length);
+		log("CANCoderList Length: " + CANCoderList.length);
+		swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME);
+	}
+
+	// A set of simple functions for the sake of adding vectors
+	/**
+	 * Returns the pythagorean theorem of two numbers
+	 * 
+	 * @param x First number
+	 * @param y Second number
+	 * @return
+	 */
+	public double pythagorean(double x, double y) {
+		return Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
+	}
+
+	/**
+	 * Returns the angle of a vector, used by joystick inputs
+	 * 
+	 * @param LR The x value of the vector
+	 * @param FB The y value of the vector
+	 * @return The angle of the vector
+	 */
+	public double getAngle(double LR, double FB) {
+		return Math.toDegrees(Math.atan2(LR, -FB));
+	}
+
+	/**
+	 * Returns whether two angles are within 90 degrees of each other, used to see if the wheels
+	 * should move backwards or not
+	 * 
+	 * @param angle1 The first angle
+	 * @param angle2 The second angle
+	 * @return If they are within 90 degrees of each other
+	 */
+	public boolean withinHalfACircle(double angle1, double angle2) {
+		angle1 = mod(angle1, 360);
+		angle2 = mod(angle2, 360);
+		if (Math.abs(angle2 - angle1) > Math.abs(angle2 + 360 - angle1)) {
+			angle2 += 360;
+		}
+		if (Math.abs(angle2 - angle1) > Math.abs(angle2 - 360 - angle1)) {
+			angle2 -= 360;
+		}
+		return Math.abs(angle2 - angle1) <= 90;
+	}
+
+	// Returns mod(d1, d2), to use to circumvent java's weird % function
+	private static double mod(double d1, double d2) {
+		return d1 % d2 + (d1 < 0 ? d2 : 0);
+	}
+
+	/**
+	 * Rounds a number based on its value and places
+	 * 
+	 * @param value The number to be rounded
+	 * @param places The number of places to round to
+	 * @return The rounded number
+	 */
+	public double round(double value, int places) {
+		double scale = Math.pow(10, places);
+		return Math.round(value * scale) / scale;
+	}
+
+	/**
+	 * Adds two vectors together
+	 * 
+	 * @param FirstMag The magnitude of the first vector
+	 * @param FirstAng The angle of the first vector
+	 * @param SecondMag The magnitude of the second vector
+	 * @param SecondAng The angle of the second vector
+	 * @return New angle of the vector
+	 */
+	public double NewAng(double FirstMag, double FirstAng, double SecondMag, double SecondAng) {
+		double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng))
+				+ SecondMag * Math.cos(Math.toRadians(SecondAng));
+		double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng))
+				+ SecondMag * Math.sin(Math.toRadians(SecondAng));
+		return round(Math.toDegrees(Math.atan2(FinalY, FinalX)), 5);
+	}
+
+	/**
+	 * Adds two vectors together
+	 * 
+	 * @param FirstMag The magnitude of the first vector
+	 * @param FirstAng The angle of the first vector
+	 * @param SecondMag The magnitude of the second vector
+	 * @param SecondAng The angle of the second vector
+	 * @return New magnitude of the vector
+	 */
+	public double NewMag(double FirstMag, double FirstAng, double SecondMag, double SecondAng) {
+		double FinalX = FirstMag * Math.cos(Math.toRadians(FirstAng))
+				+ SecondMag * Math.cos(Math.toRadians(SecondAng));
+		double FinalY = FirstMag * Math.sin(Math.toRadians(FirstAng))
+				+ SecondMag * Math.sin(Math.toRadians(SecondAng));
+		return round(Math.sqrt(Math.pow(FinalX, 2) + Math.pow(FinalY, 2)), 5);
+	}
+
+	/**
+	 * Corrects the joystick inputs to make them more accurate, currently unused
+	 * 
+	 * @param Joystick The joystick value to be corrected
+	 * @returnThe corrected joystick value
+	 */
+	public static double correctedJoysticks(double Joystick) {
+		if (Joystick >= 0)
+			return (3.0 * Math.pow(Joystick, 2) - 2.0 * Math.pow(Joystick, 3));
+		else
+			return (-1 * 3.0 * Math.pow(-1 * Joystick, 2) + 2.0 * Math.pow(-1 * Joystick, 3));
+	}
+
+	/**
+	 * Converts the angle of the joystick to the angle of the robot compared to the field by using
+	 * the gyro
+	 * 
+	 * @param angle The angle of the joystick
+	 * @param gyro The angle of the gyro
+	 * @return The angle of the robot compared to the field
+	 */
+	public static double fieldAngle(double angle, double gyro) {
+		double newAngle;
+		newAngle = angle - gyro;
+		if (newAngle < 0) {
+			newAngle = newAngle + 360;
+		}
+		if (newAngle >= 180) {
+			newAngle = newAngle - 360;
+		}
+		return newAngle;
+	}
+
+	/**
+	 * Method to correct angles for the falcon encoders
+	 * 
+	 * @param newAngle The given angle value
+	 * @param lastAngle The last angle value
+	 * @return The corrected angle value
+	 */
+	public static double newAngle(double newAngle, double lastAngle) {
+		while (newAngle < 0)
+			newAngle += 360;
+		while (newAngle < (lastAngle - 180))
+			newAngle += 360;
+		while (newAngle > (lastAngle + 180))
+			newAngle -= 360;
+		return newAngle;
+	}
+
+	/**
+	 * Sets the gyro value, used to switch between field and robot orientation
+	 * 
+	 * @param value The value to set the gyro to
+	 */
+	public void setGyro(double value) {
+		gyroValue = value;
+	}
+
+	/**
+	 * This method is used to drive the robot in 2D without turning, using the joystick values.
+	 * 
+	 * @param JoystickX The x value of the joystick
+	 * @param JoystickY The y value of the joystick
+	 */
+	public void drive2D(double JoystickX, double JoystickY) {
+		checkContextOwnership();
+		// double power = pythagorean((JoystickX), correctedJoysticks(JoystickY))/Math.sqrt(2);
+		double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY));
+		double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue);
+		log("Given angle: " + getAngle(JoystickX, JoystickY) + " || Gyro: " + gyroValue
+				+ " || New angle: " + angle);
+
+		if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontRight))) {
+			m_DriveFrontRight.set(power);
+			setFrontRightAngle(newAngle(angle, getCurrentAngle(m_SteerFrontRight)));
+		} else {
+			m_DriveFrontRight.set(-power);
+			setFrontRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontRight)));
+		}
+
+		if (withinHalfACircle(angle, getCurrentAngle(m_SteerFrontLeft))) {
+			m_DriveFrontLeft.set(power);
+			setFrontLeftAngle(newAngle(angle, getCurrentAngle(m_SteerFrontLeft)));
+		} else {
+			m_DriveFrontLeft.set(-power);
+			setFrontLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerFrontLeft)));
+		}
+
+		if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackRight))) {
+			m_DriveBackRight.set(power);
+			setBackRightAngle(newAngle(angle, getCurrentAngle(m_SteerBackRight)));
+		} else {
+			m_DriveBackRight.set(-power);
+			setBackRightAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackRight)));
+		}
+
+		if (withinHalfACircle(angle, getCurrentAngle(m_SteerBackLeft))) {
+			m_DriveBackLeft.set(power);
+			setBackLeftAngle(newAngle(angle, getCurrentAngle(m_SteerBackLeft)));
+		} else {
+			m_DriveBackLeft.set(-power);
+			setBackLeftAngle(newAngle(180 + angle, getCurrentAngle(m_SteerBackLeft)));
+		}
+	}
+
+	/**
+	 * This method is used to drive the robot in 2D without turning, using a point.
+	 * 
+	 * @param joystick The point to use for the joystick values
+	 */
+	public void drive2D(Point joystick) {
+		drive2D(joystick.getX(), joystick.getY());
+	}
+
+	/**
+	 * This method stops all of the drive motors
+	 */
+	public void stopDriveMotors() {
+		checkContextOwnership();
+		m_DriveFrontRight.stopMotor();
+		m_DriveFrontLeft.stopMotor();
+		m_DriveBackRight.stopMotor();
+		m_DriveBackLeft.stopMotor();
+	}
+
+	/**
+	 * This method stops all of the steer motors
+	 */
+	public void stopSteerMotors() {
+		checkContextOwnership();
+		m_SteerFrontRight.stopMotor();
+		m_SteerFrontLeft.stopMotor();
+		m_SteerBackRight.stopMotor();
+		m_SteerBackLeft.stopMotor();
+	}
+
+	/**
+	 * This method is the main method for driving the robot, using the joystick values.
+	 * 
+	 * @param JoystickX The x value of the joystick
+	 * @param JoystickY The y value of the joystick
+	 * @param JoystickTheta The theta value of the joystick (for turning)
+	 */
+	public void swerveDrive(double JoystickX, double JoystickY, double JoystickTheta) {
+		checkContextOwnership();
+		double power = Math.max(Math.abs(JoystickX), Math.abs(JoystickY));
+		double angle = fieldAngle(getAngle(JoystickX, JoystickY), gyroValue);
+		double frPower;
+		double flPower;
+		double brPower;
+		double blPower;
+		double frAngle;
+		double flAngle;
+		double brAngle;
+		double blAngle;
+		if (JoystickTheta >= 0) {
+			frPower = NewMag(power, angle, JoystickTheta, 45);
+			flPower = NewMag(power, angle, JoystickTheta, -45);
+			brPower = NewMag(power, angle, JoystickTheta, 135);
+			blPower = NewMag(power, angle, JoystickTheta, -135);
+			frAngle = NewAng(power, angle, JoystickTheta, 45);
+			flAngle = NewAng(power, angle, JoystickTheta, 135);
+			brAngle = NewAng(power, angle, JoystickTheta, -45);
+			blAngle = NewAng(power, angle, JoystickTheta, -135);
+		} else {
+			frPower = NewMag(power, angle, Math.abs(JoystickTheta), -135);
+			flPower = NewMag(power, angle, Math.abs(JoystickTheta), 135);
+			brPower = NewMag(power, angle, Math.abs(JoystickTheta), -45);
+			blPower = NewMag(power, angle, Math.abs(JoystickTheta), 45);
+			frAngle = NewAng(power, angle, Math.abs(JoystickTheta), -135);
+			flAngle = NewAng(power, angle, Math.abs(JoystickTheta), -45);
+			brAngle = NewAng(power, angle, Math.abs(JoystickTheta), 135);
+			blAngle = NewAng(power, angle, Math.abs(JoystickTheta), 45);
+		}
+		if (Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower)) > 1) {
+			frPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower));
+			flPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower));
+			brPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower));
+			blPower /= Math.max(Math.max(frPower, flPower), Math.max(brPower, blPower));
+		}
+
+		if (withinHalfACircle(frAngle, getCurrentAngle(m_SteerFrontRight))) {
+			m_DriveFrontRight.set(frPower);
+			setFrontRightAngle(newAngle(frAngle, getCurrentAngle(m_SteerFrontRight)));
+			log(frAngle + " " + getCurrentAngle(m_SteerFrontRight) + " Positive "
+					+ newAngle(frAngle, getCurrentAngle(m_SteerFrontRight)));
+		} else {
+			m_DriveFrontRight.set(-frPower);
+			setFrontRightAngle(newAngle(180 + frAngle, getCurrentAngle(m_SteerFrontRight)));
+			log(frAngle + " " + getCurrentAngle(m_SteerFrontRight) + " Negative "
+					+ newAngle(-frAngle, getCurrentAngle(m_SteerFrontRight)));
+		}
+
+		if (withinHalfACircle(flAngle, getCurrentAngle(m_SteerFrontLeft))) {
+			m_DriveFrontLeft.set(flPower);
+			setFrontLeftAngle(newAngle(flAngle, getCurrentAngle(m_SteerFrontLeft)));
+		} else {
+			m_DriveFrontLeft.set(-flPower);
+			setFrontLeftAngle(newAngle(180 + flAngle, getCurrentAngle(m_SteerFrontLeft)));
+		}
+
+		if (withinHalfACircle(brAngle, getCurrentAngle(m_SteerBackRight))) {
+			m_DriveBackRight.set(brPower);
+			setBackRightAngle(newAngle(brAngle, getCurrentAngle(m_SteerBackRight)));
+		} else {
+			m_DriveBackRight.set(-brPower);
+			setBackRightAngle(newAngle(180 + brAngle, getCurrentAngle(m_SteerBackRight)));
+		}
+
+		if (withinHalfACircle(blAngle, getCurrentAngle(m_SteerBackLeft))) {
+			m_DriveBackLeft.set(blPower);
+			setBackLeftAngle(newAngle(blAngle, getCurrentAngle(m_SteerBackLeft)));
+		} else {
+			m_DriveBackLeft.set(-blPower);
+			setBackLeftAngle(newAngle(180 + blAngle, getCurrentAngle(m_SteerBackLeft)));
+		}
+	}
+
+	/**
+	 * This method is used to drive the robot with swerve using a PointDir
+	 * 
+	 * @param joystick The PointDir to use for the joystick values
+	 */
+	public void swerveDrive(PointDir joystick) {
+		swerveDrive(-1 * joystick.getY(), -1 * joystick.getX(), joystick.getHeading());
+	}
+
+	/**
+	 * This method is used to simply turn the robot without moving it
+	 * 
+	 * @param Joystick The joystick value to use for turning
+	 */
+	public void turning(double Joystick) {
+		checkContextOwnership();
+		if (Joystick > 0) {
+			setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight)));
+			setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft)));
+			setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight)));
+			setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft)));
+			m_DriveFrontRight.set(Math.abs(Joystick));
+			m_DriveFrontLeft.set(Math.abs(Joystick));
+			m_DriveBackRight.set(Math.abs(Joystick));
+			m_DriveBackLeft.set(Math.abs(Joystick));
+		}
+		if (Joystick < 0) {
+			setFrontRightAngle(newAngle(-45, getCurrentAngle(m_SteerFrontRight)));
+			setFrontLeftAngle(newAngle(-135, getCurrentAngle(m_SteerFrontLeft)));
+			setBackRightAngle(newAngle(45, getCurrentAngle(m_SteerBackRight)));
+			setBackLeftAngle(newAngle(135, getCurrentAngle(m_SteerBackLeft)));
+			m_DriveFrontRight.set(Math.abs(Joystick));
+			m_DriveFrontLeft.set(Math.abs(Joystick));
+			m_DriveBackRight.set(Math.abs(Joystick));
+			m_DriveBackLeft.set(Math.abs(Joystick));
+		}
+	}
+
+	private double getCurrentAngle(MotorController motor) {
+		return Math.pow((2048.0 / 360.0 * (150.0 / 7.0)), -1) * motor.getSensorPosition();
+	}
+
+	/**
+	 * Simple encoder logging method
+	 */
+	public void logs() {
+		log("Front Right Encoder: " + getFrontRight() + " Front Left Encoder: " + getFrontLeft()
+				+ " Back Right Encoder: " + getBackRight() + " Back Left Encoder: "
+				+ getBackLeft());
+	}
+
+	/**
+	 * This method is used to set the front right encoder to the true position
+	 */
+	public void setFrontRightEncoders() {
+		m_SteerFrontRight.setSensorPosition((int) Math
+				.round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontRight.getAbsolutePosition()));
+	}
+
+	/**
+	 * This method is used to set the front left encoder to the true position
+	 */
+	public void setFrontLeftEncoders() {
+		m_SteerFrontLeft.setSensorPosition((int) Math
+				.round(2048.0 / 360.0 * (150.0 / 7.0) * e_FrontLeft.getAbsolutePosition()));
+
+	}
+
+	/**
+	 * This method is used to set the back right encoder to the true position
+	 */
+	public void setBackRightEncoders() {
+		m_SteerBackRight.setSensorPosition((int) Math
+				.round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackRight.getAbsolutePosition()));
+	}
+
+	/**
+	 * This method is used to set the back left encoder to the true position
+	 */
+	public void setBackLeftEncoders() {
+		m_SteerBackLeft.setSensorPosition((int) Math
+				.round(2048.0 / 360.0 * (150.0 / 7.0) * e_BackLeft.getAbsolutePosition()));
+	}
+
+	// To control each steering individually with a PID
+
+	/**
+	 * This method is used to set the front right steering motor to a certain angle. This uses a PID
+	 * controller.
+	 * 
+	 * @param angle The angle to set the front right wheel to
+	 */
+	public void setFrontRightAngle(double angle) {
+		// log("Angle: " + getFrontRight() + " || Motor angle: " + 360.0/ 2048.0 *
+		// m_SteerFrontRight.getSensorPosition());
+		m_SteerFrontRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle);
+	}
+
+	/**
+	 * This method is used to set the front left steering motor to a certain angle. This uses a PID
+	 * controller.
+	 * 
+	 * @param angle The angle to set the front left wheel to
+	 */
+	public void setFrontLeftAngle(double angle) {
+		// log("Angle: " + getFrontLeft() + " || Motor angle: " + Math.pow((2048.0/360.0 *
+		// (150.0/7.0)),-1) * m_SteerFrontLeft.getSensorPosition());
+		m_SteerFrontLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle);
+	}
+
+	/**
+	 * This method is used to set the back right steering motor to a certain angle. This uses a PID
+	 * controller.
+	 * 
+	 * @param angle The angle to set the back right wheel to
+	 */
+	public void setBackRightAngle(double angle) {
+		// log("Angle: " + getBackRight() + " || Motor angle: " +
+		// m_SteerBackRight.getSensorPosition());
+		m_SteerBackRight.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle);
+	}
+
+	/**
+	 * This method is used to set the back left steering motor to a certain angle. This uses a PID
+	 * controller.
+	 * 
+	 * @param angle The angle to set the back left wheel to
+	 */
+	public void setBackLeftAngle(double angle) {
+		// log("Angle: " + getBackLeft() + " || Motor angle: " +
+		// m_SteerBackLeft.getSensorPosition());
+		m_SteerBackLeft.set(ControlMode.Position, 2048.0 / 360.0 * (150.0 / 7.0) * angle);
+	}
+
+	/**
+	 * Method to configure PID values. The values were pre-tuned and are not expected to change.
+	 */
+	public void configPID() {
+		// PID for turning the various steering motors. Here is a good link to a tuning website:
+		// https://www.robotsforroboticists.com/pid-control/
+		m_SteerFrontRight.setP(0.2);
+		m_SteerFrontRight.setI(0);
+		m_SteerFrontRight.setD(0.1);
+		m_SteerFrontRight.setFF(0);
+
+		m_SteerFrontLeft.setP(0.2);
+		m_SteerFrontLeft.setI(0);
+		m_SteerFrontLeft.setD(0.1);
+		m_SteerFrontLeft.setFF(0);
+
+		m_SteerBackRight.setP(0.2);
+		m_SteerBackRight.setI(0);
+		m_SteerBackRight.setD(0.1);
+		m_SteerBackRight.setFF(0);
+
+		m_SteerBackLeft.setP(0.2);
+		m_SteerBackLeft.setI(0);
+		m_SteerBackLeft.setD(0.1);
+		// m_SteerBackLeft.setFF(0);
+
+		// pid values from sds for Flacons 500: P = 0.2 I = 0.0 D = 0.1 FF = 0.0
+
+		// Code to invert sensors if needed. Recommended to do this in phoenix tuner.
+		// m_SteerFrontRight.setSensorInverted(false);
+		// m_SteerFrontLeft.setSensorInverted(false);
+		// m_SteerBackRight.setSensorInverted(false);
+		// m_SteerBackLeft.setSensorInverted(false);
+	}
+
+	// Methods to get the encoder values, the encoders are in degrees from -180 to 180. To change
+	// that, we need to change the syntax and use getPosition()
+	public double getFrontRight() {
+		return e_FrontRight.getAbsolutePosition();
+	}
+
+	public double getFrontLeft() {
+		return e_FrontLeft.getAbsolutePosition();
+	}
+
+	public double getBackRight() {
+		return e_BackRight.getAbsolutePosition();
+	}
+
+	public double getBackLeft() {
+		return e_BackLeft.getAbsolutePosition();
+	}
+
+	public PointDir getCurrentPosition() {
+		return currentPosition;
+	}
+
+	public void setCross() {
+		checkContextOwnership();
+		setBackLeftAngle(newAngle(-45, getCurrentAngle(m_SteerBackLeft)));
+		setFrontLeftAngle(newAngle(45, getCurrentAngle(m_SteerFrontLeft)));
+		setFrontRightAngle(newAngle(135, getCurrentAngle(m_SteerFrontRight)));
+		setBackRightAngle(newAngle(-135, getCurrentAngle(m_SteerBackRight)));
+	}
+
+	public void resetCurrentPosition() {
+		swerveOdometry.setCurrentPosition(new Point(0, 0));
+	}
+
+	/**
+	 * This method is used to reset the drive encoders.
+	 */
+	public void resetDriveEncoders() {
+		m_DriveBackLeft.setSensorPosition(0);
+		m_DriveBackRight.setSensorPosition(0);
+		m_DriveFrontLeft.setSensorPosition(0);
+		m_DriveFrontRight.setSensorPosition(0);
+	}
+
+	// Odometry
+	@Override
+	public void run() {
+		currentPosition = swerveOdometry.run();
+		log(currentPosition.toString());
+	}
+}
+
+// AS
diff --git a/src/main/java/com/team766/robot/mechanisms/Grabber.java b/src/main/java/com/team766/robot/mechanisms/Grabber.java
new file mode 100644
index 00000000..08aaa418
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Grabber.java
@@ -0,0 +1,25 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+
+public class Grabber extends Mechanism{
+	
+	private MotorController wrist;
+	private SolenoidController grabber;
+
+	public Grabber(){
+		wrist = RobotProvider.instance.getMotor("wrist");
+		grabber = RobotProvider.instance.getSolenoid("grabber");
+	}
+
+	public void grabIn(){
+		grabber.set(true);
+	}
+
+	public void grabOut(){
+		grabber.set(false);
+	}
+}
diff --git a/src/main/java/com/team766/robot/mechanisms/Gyro.java b/src/main/java/com/team766/robot/mechanisms/Gyro.java
new file mode 100644
index 00000000..effee44c
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Gyro.java
@@ -0,0 +1,53 @@
+package com.team766.robot.mechanisms;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.I2C.Port;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.EncoderReader;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.MotorController;
+import com.team766.library.RateLimiter;
+import com.team766.hal.MotorController;
+import com.team766.logging.Category;
+//import edu.wpi.first.wpilibj.I2C.Port;
+//import com.team766.hal.GyroReader;
+//import com.kauailabs.navx.frc.*;
+import com.ctre.phoenix.sensors.Pigeon2;
+
+public class Gyro extends Mechanism {
+	Pigeon2 g_gyro = new Pigeon2(0, "Swervavore");
+	double[] gyroArray = new double[3];
+	private RateLimiter l_loggingRate = new RateLimiter(0.05);
+	public Gyro() {
+		loggerCategory = Category.GYRO;
+	}
+	public void resetGyro(){
+		g_gyro.setYaw(0);
+	}
+	public double getGyroPitch() {
+		double angle = g_gyro.getPitch();
+		return angle;
+		
+	}
+
+	public double getGyroYaw() {
+		double angle = -1* g_gyro.getYaw();
+		return angle;
+	}
+
+	public double getGyroRoll() {
+		double angle = g_gyro.getRoll();
+		return angle;
+	}
+
+	@Override
+	public void run() {
+		 if (l_loggingRate.next()) {
+			 gyroArray[0] = getGyroYaw();
+			 gyroArray[1] = getGyroPitch();
+			 gyroArray[2] = getGyroRoll();
+			 g_gyro.getYawPitchRoll(gyroArray);
+		 }
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java
new file mode 100644
index 00000000..ccfcaa1b
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Intake.java
@@ -0,0 +1,44 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+import com.team766.hal.wpilib.CANSparkMaxMotorController;
+import com.team766.hal.wpilib.Solenoid;
+
+public class Intake extends Mechanism {
+	
+	private MotorController bottomWheels;
+	private MotorController topBelt;
+	private SolenoidController leftPiston;
+	private SolenoidController rightPiston;
+
+	public Intake(){
+		bottomWheels = RobotProvider.instance.getMotor("intake.bottomWheels");
+		topBelt = RobotProvider.instance.getMotor("intake.topWheels");
+		leftPiston = RobotProvider.instance.getSolenoid("intake.leftPiston");
+		rightPiston = RobotProvider.instance.getSolenoid("intake.rightPiston");
+	}
+
+	public void intakeIn(){
+		bottomWheels.set(1);
+		topBelt.set(1);
+		leftPiston.set(true);
+		rightPiston.set(true);
+	}
+
+	public void intakeOut(){
+		bottomWheels.set(-1);
+		topBelt.set(-1);
+		leftPiston.set(true);
+		rightPiston.set(true);
+	}
+
+	public void intakeIdle(){
+		bottomWheels.set(0);
+		topBelt.set(0);
+		leftPiston.set(false);
+		rightPiston.set(false);
+	}
+}
diff --git a/src/main/java/com/team766/robot/mechanisms/Storage.java b/src/main/java/com/team766/robot/mechanisms/Storage.java
new file mode 100644
index 00000000..6b00e5c8
--- /dev/null
+++ b/src/main/java/com/team766/robot/mechanisms/Storage.java
@@ -0,0 +1,28 @@
+package com.team766.robot.mechanisms;
+
+import com.team766.framework.Mechanism;
+import com.team766.hal.MotorController;
+import com.team766.hal.RobotProvider;
+import com.team766.hal.SolenoidController;
+import com.team766.hal.wpilib.CANSparkMaxMotorController;
+
+public class Storage extends Mechanism {
+	
+	private MotorController belt;
+
+	public Storage(){
+		belt = RobotProvider.instance.getMotor("belt");
+	}
+
+	public void beltIn(){
+		belt.set(1);
+	}
+
+	public void beltOut(){
+		belt.set(-1);
+	}
+
+	public void beltIdle(){
+		belt.set(0);
+	}
+}
diff --git a/src/main/java/com/team766/robot/procedures/FollowPoints.java b/src/main/java/com/team766/robot/procedures/FollowPoints.java
new file mode 100644
index 00000000..aed26669
--- /dev/null
+++ b/src/main/java/com/team766/robot/procedures/FollowPoints.java
@@ -0,0 +1,365 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Procedure;
+import com.team766.framework.Context;
+import com.team766.framework.LaunchedContext;
+import com.team766.robot.Robot;
+import com.team766.library.RateLimiter;
+import com.team766.library.ValueProvider;
+import com.team766.hal.RobotProvider;
+import com.team766.odometry.Point;
+import com.team766.odometry.PointDir;
+import com.team766.hal.PositionReader;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+
+import com.team766.config.ConfigFileReader;
+import com.team766.logging.Category;
+import com.team766.logging.Severity;
+import com.team766.controllers.PIDController;
+import edu.wpi.first.wpilibj.Filesystem;
+import org.json.*;
+import com.team766.robot.constants.*;
+
+/**
+ * {@link Procedure} to follow a set of waypoints.  Waypoint files can be passed in via
+ * the {@link #FollowPoints(String)} constructor.
+ */
+public class FollowPoints extends Procedure {
+
+	//Steps combine possible data types into one object for flexibility and ease-of-use purposes
+	public static class Step {
+
+		public PointDir wayPoint;
+		public boolean criticalPoint;
+		public Procedure procedure;
+		public boolean stopRobot;
+		// ...
+
+		public Step(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) {
+			this.wayPoint = wayPoint;
+			this.procedure = procedure;
+			this.criticalPoint = criticalPoint;
+			this.stopRobot = stopRobot;
+		}
+	}
+
+	private List<Step> steps = new ArrayList<Step>();
+
+	private static PointDir currentPos = new PointDir(0.0, 0.0, 0.0);
+	private static PointDir lastPos = new PointDir(0.0, 0.0, 0.0);
+	private static PointDir lastPos2 = new PointDir(0.0, 0.0, 0.0);
+
+	private PointDir[] pointList;
+	private Procedure[] proceduresAtPoints;
+	private boolean[] criticalPointList;
+	private boolean[] stopRobotList;
+
+	private int targetNum = 0;
+	private RateLimiter followLimiter = new RateLimiter(FollowPointsInputConstants.RATE_LIMITER_TIME);
+
+	//Radius defines the radius of the circle around the robot
+	private static double radius = FollowPointsInputConstants.RADIUS;
+	private static double speed = FollowPointsInputConstants.SPEED;
+	private static PointDir driveSettings = new PointDir(0, 0, 0);
+
+	private static HashMap<String, Procedure> mapOfProcedures = new HashMap<String, Procedure>();
+
+	/*public FollowPoints() {
+		parsePointL$ist();
+		proceduresAtPoints = new Procedure[pointList.length];
+		for (int i = 0; i < proceduresAtPoints.length; i++) {
+			proceduresAtPoints[i] = new DoNothing();
+		}
+		loggerCategory = Category.AUTONOMOUS;
+	}*/
+
+	/**
+	 * Constructor to create a new FollowPoints instance.
+	 * @param file filename for the waypoints file to load and use.
+	 * @throws IOException Thrown if file is not found.
+	 */
+	public FollowPoints(String file) {
+		mapOfProcedures.put("DoNothing()", new DoNothing());
+		mapOfProcedures.put(null, new DoNothing());
+		String str;
+		Path path = Filesystem.getDeployDirectory().toPath().resolve(file);
+		try {
+			str = Files.readString(path);
+		} catch (IOException e) {
+			e.printStackTrace();
+			log(Severity.ERROR, "Could not load " + file);
+			return;
+		}
+		
+		JSONArray points = new JSONObject(str).getJSONArray("points");
+		for (int i = 0; i < points.length(); i++) {
+			JSONObject procedure = points.getJSONObject(i).getJSONObject("procedure");
+			Procedure pointProcedure = null;
+			boolean stopAtProcedure = false;
+			if (procedure != null) {
+				pointProcedure = mapOfProcedures.get(procedure.getString("name"));
+				stopAtProcedure = procedure.getBoolean("stop");
+			}
+			addStep(new PointDir(points.getJSONObject(i).getJSONArray("coordinates").getDouble(0), points.getJSONObject(i).getJSONArray("coordinates").getDouble(1), points.getJSONObject(i).getJSONArray("coordinates").getDouble(2)), points.getJSONObject(i).getBoolean("critical"), pointProcedure, stopAtProcedure);
+		}
+		addWaypoints();
+	}
+
+	/**
+	 * Method which creates a new Step object from its constituents.
+	 * @param wayPoint PointDir consisting of the x- and y- coordinates of the point, as well as the target header for the robot at that point.
+	 * @param criticalPoint Boolean determining whether the robot should travel to the point (true) or if it should use the FollowPoints circle method (false).
+	 * @param procedure Procedure for the robot to run when reaching the point. If null is inputted, it is interpreted as "doNothing()".
+	 * @param stopRobot Boolean determining whether the robot should wait for the procedure to finish before continuing on its path.
+	 */
+	private void addStep(PointDir wayPoint, boolean criticalPoint, Procedure procedure, boolean stopRobot) {
+		steps.add(new Step(wayPoint, criticalPoint, procedure, stopRobot));
+	}
+
+	/**
+	 * Default FollowPoints Constructor. Steps must be added in-code.
+	 */
+	public FollowPoints() {
+		addStep(new PointDir(0,0, 0), false, new DoNothing(), false);
+		addStep(new PointDir(2,0, 90), false, null /* don't execute procedure */, false);
+		addStep(new PointDir(2,2, 0), false, new DoNothing(), false);
+		addStep(new PointDir(0,2, 90), false, null /* don't execute procedure */, false);
+		addStep(new PointDir(0,0, 0), true, new DoNothing(), false);
+		addWaypoints();
+	}
+
+	/**
+	 * When using steps (in the default constructor), this sets up the arrays to be used by the FollowPoints method.
+	 */
+	private void addWaypoints() {
+		pointList = new PointDir[steps.size()];
+		proceduresAtPoints = new Procedure[steps.size()];
+		stopRobotList = new boolean[pointList.length];
+		criticalPointList = new boolean[pointList.length];
+		for (int i = 0; i < steps.size(); i++) {
+			if (steps.get(i).wayPoint == null) continue;
+			else {
+				pointList[i] = steps.get(i).wayPoint;
+			}
+			if (steps.get(i).procedure != null) {
+				proceduresAtPoints[i] = steps.get(i).procedure;
+				// run procedure
+				// TODO: make sure we're handling contexts correctly
+			} else {
+				proceduresAtPoints[i] = new DoNothing();
+			}
+			criticalPointList[i] = steps.get(i).criticalPoint;
+
+			stopRobotList[i] = steps.get(i).stopRobot;
+		}
+	}
+
+
+	/**
+	 * Constructor which takes an array of points.
+	 * @param points Array of PointDir objects for the robot to follow, consisting of x- and y- coordinates, as well as a target header for the robot.
+	 */
+	public FollowPoints(PointDir[] points) {
+		pointList = points;
+		proceduresAtPoints = new Procedure[pointList.length];
+		for (int i = 0; i < proceduresAtPoints.length; i++) {
+			proceduresAtPoints[i] = new DoNothing();
+		}
+		criticalPointList = new boolean[pointList.length];
+		stopRobotList = new boolean[pointList.length];
+		loggerCategory = Category.AUTONOMOUS;
+	}
+
+	//Takes pairs of points from pointDoubles (set in the config file) and converts them to Points, which are placed in pointList.
+	/*private void parsePointList() {
+		Double[] pointDoubles = ConfigFileReader.getInstance().getDoubles("trajectory.points").get();
+		pointList = new PointDir[pointDoubles.length / 2];
+		double pointX = 0;
+		double pointY = 0;
+		for (int i = 0; i < pointDoubles.length / 2 * 2; i++) {
+			if (i % 2 == 0)
+				pointX = pointDoubles[i];
+			else {
+				pointY = pointDoubles[i];
+				pointList[i / 2] = new PointDir(pointX, pointY);
+			}
+		}
+	}*/
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.drive);
+		context.takeOwnership(Robot.gyro);
+		log("Starting FollowPoints");
+		//This resetCurrentPosition() call makes FollowPoints() robot-oriented instead of field-oriented
+		//If we need to make this method field-oriented, just remove this line
+		Robot.drive.resetCurrentPosition();
+		targetNum = 0;
+
+		for (int i = 0; i < pointList.length; i++) {
+			log(pointList[i].toString());
+		}
+		if (pointList.length > 0) {
+			Point targetPoint = new Point(0.0, 0.0);
+			currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading());
+			while (targetNum < pointList.length - 1 ||  !passedPoint(pointList[pointList.length - 1])) {
+				if (followLimiter.next()) {
+					lastPos2 = lastPos.clone();
+					lastPos = currentPos.clone();
+					currentPos.set(Robot.drive.getCurrentPosition().getX(), Robot.drive.getCurrentPosition().getY(), Robot.drive.getCurrentPosition().getHeading());
+					//If the next point is a critical point, the robot will wait until it has passed that point for it to move to the next point
+					//Otherwise, it uses the checkIntersection() method to follow the circle
+					if (criticalPointList[targetNum]? (targetNum < pointList.length - 1 && passedPoint(pointList[targetNum])) : checkIntersection(pointList)) {
+						if (proceduresAtPoints.length < targetNum) {
+							if (stopRobotList[targetNum]) {
+								Robot.drive.setCross();
+								context.waitFor(context.startAsync(proceduresAtPoints[targetNum])); 
+							} else {
+								context.startAsync(proceduresAtPoints[targetNum]);
+							}
+						}
+						targetNum++;
+						log("Going to Next Point! " + pointList[targetNum]);
+					}
+					targetPoint = selectTargetPoint(targetNum, pointList);
+					//double diff = currentPos.getAngleDifference(targetPoint);
+					//Robot.drive.setDrivePower(straightVelocity + Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity), straightVelocity - Math.signum(diff) * Math.min(Math.abs(diff) * theBrettConstant, 1 - straightVelocity));
+					
+					Robot.drive.setGyro(Robot.gyro.getGyroYaw());
+					driveSettings.set(currentPos.scaleVector(targetPoint, speed), rotationSpeed(-Robot.gyro.getGyroYaw(), pointList[targetNum].getHeading()));
+					Robot.drive.swerveDrive(driveSettings);
+					//log("Current Position: " + currentPos.toString());
+					//log("Target Point: " + targetPoint.toString());
+					//log("Unit Vector: " + new PointDir(currentPos.scaleVector(targetPoint, speed), rotationSpeed(Robot.gyro.getGyroYaw(), pointList[targetNum].getHeading())).toString());
+
+					context.yield();
+				} else {
+					updateRotation();
+				}
+			}
+			Robot.drive.drive2D(0, 0);
+			Robot.drive.setCross();
+			log("Finished method!");
+		} else {
+			log("No points!");
+		}
+	}
+
+	/**
+	 * Method which keeps updating how much the robot should turn between rateLimiter calls.
+	 */
+	public void updateRotation() {
+		Robot.drive.setGyro(Robot.gyro.getGyroYaw());
+		driveSettings.setHeading(rotationSpeed(-Robot.gyro.getGyroYaw(), pointList[targetNum].getHeading()));
+		Robot.drive.swerveDrive(driveSettings);
+	}
+
+	/**
+	 * Method which returns whether the circle around the robot intersects the line connecting the two next points.
+	 * @param pointList The list of points the robot is following.
+	 * @return Boolean: whether or not the circle with given radius centered at currentPos intersects with the line between the next two points.
+	 */
+	private boolean checkIntersection(Point[] pointList) {
+		double a;
+		double b;
+		double c;
+		double slope;
+		if (targetNum < pointList.length - 1) {
+			slope = pointList[targetNum].slope(pointList[targetNum + 1]);
+			a = slope * slope + 1;
+			b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum].getX() + 2 * slope * pointList[targetNum].getY() - 2 * currentPos.getY() * slope;
+			c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum].getX() * pointList[targetNum].getX() - 2 * slope * pointList[targetNum].getX() * pointList[targetNum].getY() + pointList[targetNum].getY() * pointList[targetNum].getY() + 2 * currentPos.getY() * slope * pointList[targetNum].getX() - 2 * currentPos.getY() * pointList[targetNum].getY() + currentPos.getY() * currentPos.getY() - radius * radius;
+			if (b * b - 4 * a * c > 0) {
+				return true;                                        
+			}
+		}
+		return false;
+	}
+
+	/**
+	 * Method returning which point the robot should move towards.
+	 * @param targetNum The id value of the next point in pointList.
+	 * @param currentPos The current position of the robot.
+	 * @return If the circle around the robot intersects the line connecting the previous and next points, returns whichever intersection point is closest to the next point. Otherwise, returns the next point.
+	 */
+	private static Point selectTargetPoint(int targetNum, Point[] pointList) {
+		double a;
+		double b;
+		double c;
+		double slope;
+		double potentialX1;
+		double potentialX2;
+		double potentialY1;
+		double potentialY2;
+		Point potentialPoint1 = new Point(0.0, 0.0);
+		Point potentialPoint2 = new Point(0.0, 0.0);
+		if (targetNum == 0) {
+			return pointList[0];
+		} else {
+			slope = pointList[targetNum - 1].slope(pointList[targetNum]);
+			a = slope * slope + 1;
+			b = -2 * currentPos.getX() - 2 * slope * slope * pointList[targetNum - 1].getX() + 2 * slope * pointList[targetNum - 1].getY() - 2 * currentPos.getY() * slope;
+			c = currentPos.getX() * currentPos.getX() + slope * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getX() - 2 * slope * pointList[targetNum - 1].getX() * pointList[targetNum - 1].getY() + pointList[targetNum - 1].getY() * pointList[targetNum - 1].getY() + 2 * currentPos.getY() * slope * pointList[targetNum - 1].getX() - 2 * currentPos.getY() * pointList[targetNum - 1].getY() + currentPos.getY() * currentPos.getY() - radius * radius;
+			if (b * b - 4 * a * c < 0) {
+				return pointList[targetNum];
+			} else {
+				potentialX1 = (-1 * b + Math.sqrt(b * b - 4 * a * c))/ (2 * a);
+				potentialX2 = (-1 * b - Math.sqrt(b * b - 4 * a * c))/ (2 * a);
+				potentialY1 = slope * (potentialX1 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY();
+				potentialY2 = slope * (potentialX2 - pointList[targetNum - 1].getX()) + pointList[targetNum - 1].getY();
+				potentialPoint1.set(potentialX1, potentialY1);
+				potentialPoint2.set(potentialX2, potentialY2);
+				if (potentialPoint1.distance(pointList[targetNum]) <= potentialPoint2.distance(pointList[targetNum])) {
+					return potentialPoint1;
+				} else {
+					return potentialPoint2;
+				}
+			}
+		}
+	}
+
+	//Returns if the robot has passed a certain point
+	/**
+	 * Method returning whether the robot has passed a given point.
+	 * @param P Point which this method determines has been passed.
+	 * @return If the robot is within a certain distance of P, and the distance to P is increasing, returns true. Otherwise, returns false.
+	 */
+	private boolean passedPoint(Point P) {
+		//log(currentPos + " " + P + " " + currentPos.distance(P) + " " + ((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.2) ? " true" : " false"));
+		//log(((currentPos.distance(P) > lastPos.distance(P) && currentPos.distance(P) <= 0.4)) ? "true " : "false " + targetNum);
+		return (currentPos.distance(P) <= 0.03 || (currentPos.distance(P) <= 0.15 && currentPos.distance(P) > lastPos.distance(P) && lastPos.distance(P) > lastPos2.distance(P)));
+	}
+
+	/**
+	 * Method which returns how much the robot should turn to reach its target heading.
+	 * @param currentRot The current heading of the robot.
+	 * @param targetRot The target heading of the robot.
+	 * @return Returns a value between -1 and 1 corresponding to how much the robot should turn to reach the target point.
+	 */
+	private double rotationSpeed(double currentRot, double targetRot) {
+		double maxSpeed = FollowPointsInputConstants.MAX_ROTATION_SPEED;
+		double angleDistanceForMaxSpeed = FollowPointsInputConstants.ANGLE_DISTANCE_FOR_MAX_SPEED;
+		currentRot = mod(currentRot, 360);
+		targetRot = mod(targetRot, 360);
+		if (Math.abs(targetRot - currentRot) > Math.abs(targetRot + 360 - currentRot)) {
+			targetRot += 360;
+		}
+		if (Math.abs(targetRot - currentRot) > Math.abs(targetRot - 360 - currentRot)) {
+			targetRot -= 360;
+		}
+		if (Math.abs(targetRot - currentRot) <= angleDistanceForMaxSpeed) {
+			return (currentRot - targetRot) / angleDistanceForMaxSpeed * maxSpeed;
+		}
+		return maxSpeed * Math.signum(currentRot - targetRot);
+	}
+
+	//Returns mod(d1, d2), to use to circumvent java's weird % function
+	private static double mod(double d1, double d2) {
+		return d1 % d2 + (d1 < 0 ? d2 : 0);
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/com/team766/robot/procedures/setCross.java b/src/main/java/com/team766/robot/procedures/setCross.java
new file mode 100644
index 00000000..e481c027
--- /dev/null
+++ b/src/main/java/com/team766/robot/procedures/setCross.java
@@ -0,0 +1,16 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Context;
+import com.team766.framework.Procedure;
+import com.team766.robot.Robot;
+
+public class setCross extends Procedure { 
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.drive);
+		Robot.drive.stopDriveMotors();
+		Robot.drive.stopSteerMotors();
+		Robot.drive.setCross();
+	}
+	
+}
diff --git a/src/main/proto/com/team766/logging/logging.proto b/src/main/proto/com/team766/logging/logging.proto
index 40dc4b72..46dca665 100644
--- a/src/main/proto/com/team766/logging/logging.proto
+++ b/src/main/proto/com/team766/logging/logging.proto
@@ -37,6 +37,8 @@ enum Category {
 	TRAJECTORY = 9;
 	OPERATOR_INTERFACE = 10;
 	DRIVE = 11;
+	GYRO = 12;
+	ODOMETRY = 13;
 }
 
 // `Value` represents a dynamically typed value which can be either a number,
@@ -60,4 +62,4 @@ message LogValue {
 
 message LogListValue {
 	repeated LogValue element = 1;
-}
\ No newline at end of file
+}