diff --git a/HVN-ON-EARTH/LemLib@0.4.7.zip b/HVN-ON-EARTH/LemLib@0.4.7.zip
new file mode 100644
index 0000000..435b49f
Binary files /dev/null and b/HVN-ON-EARTH/LemLib@0.4.7.zip differ
diff --git a/HVN-ON-EARTH/gif-pros-asset@1.0.1.zip b/HVN-ON-EARTH/gif-pros-asset@1.0.1.zip
new file mode 100644
index 0000000..3a6c56c
Binary files /dev/null and b/HVN-ON-EARTH/gif-pros-asset@1.0.1.zip differ
diff --git a/HVN-ON-EARTH/include/Wings.h b/HVN-ON-EARTH/include/Wings.h
new file mode 100644
index 0000000..a1b64d8
--- /dev/null
+++ b/HVN-ON-EARTH/include/Wings.h
@@ -0,0 +1,22 @@
+#pragma once
+#ifndef _Wings_h
+#define _Wings_h_
+
+#include "main.h"
+
+class Wings {
+    public:
+        Wings();
+        void open();
+        void close();
+        void toggleLeft(int value);
+        void toggleRight(int value);
+        void openFor(double duration);
+        int getState();
+    
+    private:
+        int left_wing_state;
+        int right_wing_state;
+};
+
+#endif
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/api.h b/HVN-ON-EARTH/include/api.h
index 7e92319..9ddcdac 100644
--- a/HVN-ON-EARTH/include/api.h
+++ b/HVN-ON-EARTH/include/api.h
@@ -77,4 +77,6 @@
 #include "pros/link.hpp"
 #endif
 
+#include "asset.h"
+
 #endif  // _PROS_API_H_
diff --git a/HVN-ON-EARTH/include/ary-lib/PID.hpp b/HVN-ON-EARTH/include/ary-lib/PID.hpp
new file mode 100644
index 0000000..3d5bb91
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/PID.hpp
@@ -0,0 +1,184 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include "ary-lib/util.hpp"
+#include "api.h"
+
+class PID {
+ public:
+  /**
+   * Default constructor.
+   */
+  PID();
+
+  /**
+   * Constructor with constants.
+   *
+   * \param p
+   *        kP
+   * \param i
+   *        ki
+   * \param d
+   *        kD
+   * \param p_start_i
+   *        error value that i starts within
+    * \param name
+   *        std::string of name that prints 
+   */
+  PID(double p, double i = 0, double d = 0, double start_i = 0, std::string name = "");
+
+  /**
+   * Set constants for PID.
+   *
+   * \param p
+   *        kP
+   * \param i
+   *        ki
+   * \param d
+   *        kD
+   * \param p_start_i
+   *        error value that i starts within
+   */
+  void set_constants(double p, double i = 0, double d = 0, double p_start_i = 0);
+
+  /**
+   * Struct for constants.
+   */
+  struct Constants {
+    double kp;
+    double ki;
+    double kd;
+    double start_i;
+  };
+
+  /**
+   * Struct for exit condition.
+   */
+  struct exit_condition_ {
+    int small_exit_time = 0;
+    double small_error = 0;
+    int big_exit_time = 0;
+    double big_error = 0;
+    int velocity_exit_time = 0;
+    int mA_timeout = 0;
+  };
+
+  /**
+   * Set's constants for exit conditions.
+   *
+   * \param p_small_exit_time
+   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   * \param p_small_error
+   *        Sets smalL_error. Timer will start when error is within this.
+   * \param p_big_exit_time
+   *        Sets big_exit_time.  Timer for to exit within big_error.
+   * \param p_big_error
+   *        Sets big_error. Timer will start when error is within this.
+   * \param p_velocity_exit_time
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   */
+  void set_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time = 0, double p_big_error = 0, int p_velocity_exit_time = 0, int p_mA_timeout = 0);
+
+  /**
+   * Set's target.
+   *
+   * \param target
+   *        Target for PID.
+   */
+  void set_target(double input);
+
+  /**
+   * Computes PID.
+   *
+   * \param current
+   *        Current sensor library.
+   */
+  double calculate(double current);
+
+  /**
+   * Returns target value.
+   */
+  double get_target();
+
+  /**
+   * Returns constants.
+   */
+  Constants get_constants();
+
+  /**
+   * Resets all variables to 0.  This does not reset constants.
+   */
+  void reset_variables();
+
+  /**
+   * Constants
+   */
+  Constants constants;
+
+  /**
+   * Exit
+   */
+  exit_condition_ exit;
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ary::exit_output exit_condition(bool print = false);
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param sensor
+   *        A pros motor on your mechanism.
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ary::exit_output exit_condition(pros::Motor sensor, bool print = false);
+
+  /**
+   * Iterative exit condition for PID.
+   *
+   * \param sensor
+   *        Pros motors on your mechanism.
+   * \param print = false
+   *        if true, prints when complete.
+   */
+  ary::exit_output exit_condition(std::vector<pros::Motor> sensor, bool print = false);
+
+  /**
+   * Sets the name of the PID that prints during exit conditions. 
+   *
+   * \param name
+   *        a string that is the name you want to print
+   */
+  void set_name(std::string name);
+
+  /**
+   * PID variables. 
+   */
+  double output;
+  double cur;
+  double error;
+  double target;
+  double lastError;
+  double integral;
+  double derivative;
+  long time;
+  long prev_time;
+
+ private:
+  int i = 0, j = 0, k = 0, l = 0;
+  bool is_mA = false;
+  void reset_timers();
+  std::string name;
+  bool is_name = false;
+  void print_exit(ary::exit_output exit_type);
+};
diff --git a/HVN-ON-EARTH/include/ary-lib/api.hpp b/HVN-ON-EARTH/include/ary-lib/api.hpp
new file mode 100644
index 0000000..8d8709f
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/api.hpp
@@ -0,0 +1,14 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include "ary-lib/PID.hpp"
+#include "ary-lib/auton.hpp"
+#include "ary-lib/auton_selector.hpp"
+#include "ary-lib/drive/drive.hpp"
+#include "ary-lib/autonselector.hpp"
+#include "ary-lib/util.hpp"
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/ary-lib/auton.hpp b/HVN-ON-EARTH/include/ary-lib/auton.hpp
new file mode 100644
index 0000000..448e1f5
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/auton.hpp
@@ -0,0 +1,19 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+#include <functional>
+#include <iostream>
+
+class Auton {
+ public:
+  Auton();
+  Auton(std::string, std::function<void()>);
+  std::string Name;
+  std::function<void()> auton_call;
+
+ private:
+};
diff --git a/HVN-ON-EARTH/include/ary-lib/auton_selector.hpp b/HVN-ON-EARTH/include/ary-lib/auton_selector.hpp
new file mode 100644
index 0000000..8d6fdd8
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/auton_selector.hpp
@@ -0,0 +1,23 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+#include <tuple>
+
+#include "ary-lib/auton.hpp"
+
+using namespace std;
+class AutonSelector {
+ public:
+  std::vector<Auton> Autons;
+  int current_auton_page;
+  int auton_count;
+  AutonSelector();
+  AutonSelector(std::vector<Auton> autons);
+  void call_selected_auton();
+  void print_selected_auton();
+  void add_autons(std::vector<Auton> autons);
+};
diff --git a/HVN-ON-EARTH/include/ary-lib/autonselector.hpp b/HVN-ON-EARTH/include/ary-lib/autonselector.hpp
new file mode 100644
index 0000000..580f896
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/autonselector.hpp
@@ -0,0 +1,64 @@
+    /*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include "ary-lib/auton_selector.hpp"
+#include "api.h"
+
+namespace ary {
+namespace autonselector {
+extern AutonSelector auton_selector;
+/**
+ * Sets sd card to current page.
+ */
+void init_auton_selector();
+
+/**
+ * Sets the sd card to current page.
+ */
+void update_auto_sd();
+
+/**
+ * Increases the page by 1.
+ */
+void page_up();
+
+/**
+ * Decreases the page by 1.
+ */
+void page_down();
+
+/**
+ * Initializes LLEMU and sets up callbacks for auton selector.
+ */
+void initialize();
+
+/**
+ * Wrapper for pros::lcd::shutdown.
+ */
+void shutdown();
+
+extern bool turn_off;
+
+extern pros::ADIDigitalIn* left_limit_switch;
+extern pros::ADIDigitalIn* right_limit_switch;
+/**
+ * Initialize two limitswithces to change pages on the lcd
+ *
+ * @param left_limit_port
+ *        port for the left limit switch
+ * @param right_limit_port
+ *        port for the right limit switch
+ */
+void limit_switch_lcd_initialize(pros::ADIDigitalIn* right_limit, pros::ADIDigitalIn* left_limit = nullptr);
+
+/**
+ * pre_auto_task
+ */
+void limitSwitchTask();
+} 
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/ary-lib/drive/drive.hpp b/HVN-ON-EARTH/include/ary-lib/drive/drive.hpp
new file mode 100644
index 0000000..1affc95
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/drive/drive.hpp
@@ -0,0 +1,856 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include <functional>
+#include <iostream>
+#include <tuple>
+#include <cmath>
+
+#include "ary-lib/PID.hpp"
+#include "ary-lib/util.hpp"
+#include "pros/motors.h"
+
+#define VELOCITY_TO_VOLTS_LIN 196.001723856
+#define VELOCITY_TO_VOLTS_ANG 127 / 0.796332147963
+
+#define LINEAR_MAX_VEL 0.647953484803
+#define LINEAR_FWD_ACCEL 0.647953484803 / 26
+#define LINEAR_FWD_DECEL 0.647953484803 / 18
+#define LINEAR_REV_ACCEL 0.647953484803 / 18
+#define LINEAR_REV_DECEL 0.647953484803 / 32
+
+#define ANGULAR_MAX_VEL 0.796332147963
+#define ANGULAR_FWD_ACCEL 0.796332147963 / 40
+#define ANGULAR_FWD_DECEL 0.796332147963 / 40
+#define ANGULAR_REV_ACCEL 0.796332147963 / 40
+#define ANGULAR_REV_DECEL 0.796332147963 / 40
+
+using namespace ary;
+
+class Drive {
+ public:
+  /**
+   * Joysticks will return 0 when they are within this number.  Set with set_joystick_threshold()
+   */
+  int JOYSTICK_THRESHOLD;
+  /**
+   * Global current brake mode.
+   */
+  pros::motor_brake_mode_e_t CURRENT_BRAKE = pros::E_MOTOR_BRAKE_COAST;
+
+  /**
+   * Global current mA.
+   */
+  int CURRENT_MA = 2500;
+
+  /**
+   * Current swing type.
+   */
+  e_swing current_swing;
+
+  /**
+   * Vector of pros motors for the left chassis.
+   */
+  std::vector<pros::Motor> left_motors;
+
+  /**
+   * Vector of pros motors for the right chassis.
+   */
+  std::vector<pros::Motor> right_motors;
+
+  /**
+   * Vector of pros motors that are disconnected from the drive.
+   */
+  std::vector<int> pto_active;
+
+  /**
+   * Inertial sensor.
+   */
+  pros::Imu imu;
+
+  /**
+   * Left tracking wheel.
+   */
+  pros::ADIEncoder left_tracker;
+
+  /**
+   * Right tracking wheel.
+   */
+  pros::ADIEncoder right_tracker;
+
+  /**
+   * Left rotation tracker.
+   */
+  pros::Rotation left_rotation;
+
+  /**
+   * Right rotation tracker.
+   */
+  pros::Rotation right_rotation;
+
+  /**
+   * PID objects.
+   */
+  PID headingPID;
+  PID turnPID;
+  PID forward_drivePID;
+  PID leftPID;
+  PID rightPID;
+  PID backward_drivePID;
+  PID swingPID;
+
+  /**
+   * Current mode of the drive.
+   */
+  e_mode mode;
+
+  /**
+   * Sets current mode of drive.
+   */
+  void set_mode(e_mode p_mode);
+
+  /**
+   * Returns current mode of drive.
+   */
+  e_mode get_mode();
+
+  /**
+   * Calibrates imu and initializes sd card to curve.
+   */
+  void initialize();
+
+  /**
+   * Tasks for autonomous.
+   */
+  pros::Task ez_auto;
+
+  /**
+   * Creates a Drive Controller using internal encoders.
+   *
+   * \param left_motor_ports
+   *        Input {1, -2...}.  Make ports negative if reversed!
+   * \param right_motor_ports
+   *        Input {-3, 4...}.  Make ports negative if reversed!
+   * \param imu_port
+   *        Port the IMU is plugged into.
+   * \param wheel_diameter
+   *        Diameter of your drive wheels.  Remember 4" is 4.125"!
+   * \param ticks
+   *        Motor cartridge RPM
+   * \param ratio
+   *        External gear ratio, wheel gear / motor gear.
+   */
+  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio);
+
+  /**
+   * Creates a Drive Controller using encoders plugged into the brain.
+   *
+   * \param left_motor_ports
+   *        Input {1, -2...}.  Make ports negative if reversed!
+   * \param right_motor_ports
+   *        Input {-3, 4...}.  Make ports negative if reversed!
+   * \param imu_port
+   *        Port the IMU is plugged into.
+   * \param wheel_diameter
+   *        Diameter of your sensored wheels.  Remember 4" is 4.125"!
+   * \param ticks
+   *        Ticks per revolution of your encoder.
+   * \param ratio
+   *        External gear ratio, wheel gear / sensor gear.
+   * \param left_tracker_ports
+   *        Input {1, 2}.  Make ports negative if reversed!
+   * \param right_tracker_ports
+   *        Input {3, 4}.  Make ports negative if reversed!
+   */
+  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports);
+
+  /**
+   * Creates a Drive Controller using encoders plugged into a 3 wire expander.
+   *
+   * \param left_motor_ports
+   *        Input {1, -2...}.  Make ports negative if reversed!
+   * \param right_motor_ports
+   *        Input {-3, 4...}.  Make ports negative if reversed!
+   * \param imu_port
+   *        Port the IMU is plugged into.
+   * \param wheel_diameter
+   *        Diameter of your sensored wheels.  Remember 4" is 4.125"!
+   * \param ticks
+   *        Ticks per revolution of your encoder.
+   * \param ratio
+   *        External gear ratio, wheel gear / sensor gear.
+   * \param left_tracker_ports
+   *        Input {1, 2}.  Make ports negative if reversed!
+   * \param right_tracker_ports
+   *        Input {3, 4}.  Make ports negative if reversed!
+   * \param expander_smart_port
+   *        Port the expander is plugged into.
+   */
+  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ticks, double ratio, std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port);
+
+  /**
+   * Creates a Drive Controller using rotation sensors.
+   *
+   * \param left_motor_ports
+   *        Input {1, -2...}.  Make ports negative if reversed!
+   * \param right_motor_ports
+   *        Input {-3, 4...}.  Make ports negative if reversed!
+   * \param imu_port
+   *        Port the IMU is plugged into.
+   * \param wheel_diameter
+   *        Diameter of your sensored wheels.  Remember 4" is 4.125"!
+   * \param ratio
+   *        External gear ratio, wheel gear / sensor gear.
+   * \param left_tracker_port
+   *        Make ports negative if reversed!
+   * \param right_tracker_port
+   *        Make ports negative if reversed!
+   */
+  Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports, int imu_port, double wheel_diameter, double ratio, int left_rotation_port, int right_rotation_port);
+
+  /**
+   * Sets drive defaults.
+   */
+  void set_defaults();
+
+  /////
+  //
+  // User Control
+  //
+  /////
+
+  /**
+   * Sets the chassis to controller joysticks using tank control.  Run is usercontrol.
+   * This passes the controller through the curve functions, but is disabled by default.  Use toggle_controller_curve_modifier() to enable it.
+   */
+  void tank_control();
+
+  /**
+   * Sets the chassis to controller joysticks using standard arcade control.  Run is usercontrol.
+   * This passes the controller through the curve functions, but is disabled by default.  Use toggle_controller_curve_modifier() to enable it.
+   *
+   * \param stick_type
+   *        ez::SINGLE or ez::SPLIT control
+   * \param curve_type
+   */
+  void arcade_standard(e_type stick_type, e_curve_type curve_type);
+
+  /**
+   * Sets the chassis to controller joysticks using flipped arcade control.  Run is usercontrol.
+   * This passes the controller through the curve functions, but is disabled by default.  Use toggle_controller_curve_modifier() to enable it.
+   *
+   * \param stick_type
+   *        ez::SINGLE or ez::SPLIT control
+   */
+  void arcade_flipped(e_type stick_type);
+
+  /**
+   * Initializes left and right curves with the SD card, reccomended to run in initialize().
+   */
+  void init_curve_sd();
+
+  /**
+   * Sets the default joystick curves.
+   *
+   * \param left
+   *        Left default curve.
+   * \param right
+   *        Right default curve.
+   */
+  void set_curve_default(double left, double right = 0);
+
+  /**
+   * Runs a P loop on the drive when the joysticks are released.
+   *
+   * \param kp
+   *        Constant for the p loop.
+   */
+  void set_active_brake(double kp);
+
+  void set_cata_active_brake(double kp);
+
+  double get_cata_active_brake();
+
+  /**
+   * Enables/disables modifying the joystick input curves with the controller.  True enables, false disables.
+   *
+   * \param input
+   *        bool input
+   */
+  void toggle_modify_curve_with_controller(bool toggle);
+
+  /**
+   * Sets buttons for modifying the left joystick curve.
+   *
+   * \param decrease
+   *        a pros button enumerator
+   * \param increase
+   *        a pros button enumerator
+   */
+  void set_left_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase);
+
+  /**
+   * Sets buttons for modifying the right joystick curve.
+   *
+   * \param decrease
+   *        a pros button enumerator
+   * \param increase
+   *        a pros button enumerator
+   */
+  void set_right_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase);
+
+  void set_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase);
+
+  /**
+   * Outputs a curve from 5225A In the Zone.  This gives more control over the robot at lower speeds.  https://www.desmos.com/calculator/rcfjjg83zx
+   *
+   * \param x
+   *        joystick input
+   */
+  double left_curve_function(double x);
+
+  /**
+   * Outputs a curve from 5225A In the Zone.  This gives more control over the robot at lower speeds.  https://www.desmos.com/calculator/rcfjjg83zx
+   *
+   * \param x
+   *        joystick input
+   */
+  double right_curve_function(double x);
+
+  /**
+   * Sets a new threshold for the joystick.  The joysticks wil not return a value if they are within this.
+   *
+   * \param threshold
+   *        new threshold
+   */
+  void set_joystick_threshold(int threshold);
+  
+  void set_joystick_drivescale(double scaleval);
+
+  void set_joystick_turnscale(double scaleval);
+
+  /**
+   * Resets drive sensors at the start of opcontrol.
+   */
+  void reset_drive_sensors_opcontrol();
+
+  /**
+   * Sets minimum slew distance constants.
+   *
+   * \param l_stick
+   *        input for left joystick
+   * \param r_stick
+   *        input for right joystick
+   */
+  void joy_thresh_opcontrol(int l_stick, int r_stick);
+
+  /////
+  //
+  // PTO
+  //
+  /////
+
+  /**
+   * Checks if the motor is currently in pto_list.  Returns true if it's already in pto_list.
+   *
+   * \param check_if_pto
+   *        motor to check.
+   */
+  bool pto_check(pros::Motor check_if_pto);
+
+  /**
+   * Adds motors to the pto list, removing them from the drive.
+   *
+   * \param pto_list
+   *        list of motors to remove from the drive.
+   */
+  void pto_add(std::vector<pros::Motor> pto_list);
+
+  /**
+   * Removes motors from the pto list, adding them to the drive.  You cannot use the first index in a pto.
+   *
+   * \param pto_list
+   *        list of motors to add to the drive.
+   */
+  void pto_remove(std::vector<pros::Motor> pto_list);
+
+  /**
+   * Adds/removes motors from drive.  You cannot use the first index in a pto.
+   *
+   * \param pto_list
+   *        list of motors to add/remove from the drive.
+   * \param toggle
+   *        if true, adds to list.  if false, removes from list.
+   */
+  void pto_toggle(std::vector<pros::Motor> pto_list, bool toggle);
+
+  /////
+  //
+  // PROS Wrapers
+  //
+  /////
+
+  /**
+   * Sets the chassis to voltage
+   *
+   * \param left
+   *        voltage for left side, -127 to 127
+   * \param right
+   *        voltage for right side, -127 to 127
+   */
+  void set_tank(int left, int right);
+
+  /**
+   * Changes the way the drive behaves when it is not under active user control
+   *
+   * \param brake_type
+   *        the 'brake mode' of the motor e.g. 'pros::E_MOTOR_BRAKE_COAST' 'pros::E_MOTOR_BRAKE_BRAKE' 'pros::E_MOTOR_BRAKE_HOLD'
+   */
+  void set_drive_brake(pros::motor_brake_mode_e_t brake_type);
+
+  /**
+   * Sets the limit for the current on the drive.
+   *
+   * \param mA
+   *        input in milliamps
+   */
+  void set_drive_current_limit(int mA);
+
+  /**
+   * Toggles set drive in autonomous. True enables, false disables.
+   */
+  void toggle_auto_drive(bool toggle);
+
+  /**
+   * Toggles printing in autonomous. True enables, false disables.
+   */
+  void toggle_auto_print(bool toggle);
+
+  /////
+  //
+  // Telemetry
+  //
+  /////
+
+  /**
+   * The position of the right motor.
+   */
+  int right_sensor();
+
+  /**
+   * The velocity of the right motor.
+   */
+  int right_velocity();
+
+  /**
+   * The watts of the right motor.
+   */
+  double right_mA();
+
+  /**
+   * Return TRUE when the motor is over current.
+   */
+  bool right_over_current();
+
+  /**
+   * The position of the left motor.
+   */
+  int left_sensor();
+
+  /**
+   * The velocity of the left motor.
+   */
+  int left_velocity();
+
+  /**
+   * The watts of the left motor.
+   */
+  double left_mA();
+
+  /**
+   * Return TRUE when the motor is over current.
+   */
+  bool left_over_current();
+
+  /**
+   * Reset all the chassis motors, reccomended to run at the start of your autonomous routine.
+   */
+  void reset_drive_sensor();
+
+  /**
+   * Resets the current gyro value.  Defaults to 0, reccomended to run at the start of your autonomous routine.
+   *
+   * \param new_heading
+   *        New heading value.
+   */
+  void reset_gyro(double new_heading = 0);
+
+  /**
+   * Resets the imu so that where the drive is pointing is zero in set_drive_pid(turn)
+   */
+  double get_gyro();
+
+  /**
+   * Calibrates the IMU, reccomended to run in initialize().
+   *
+   * \param run_loading_animation
+   *        bool for running loading animation
+   */
+  bool imu_calibrate(bool run_loading_animation = true);
+
+  /**
+   * Loading display while the IMU calibrates.
+   */
+  void imu_loading_display(int iter);
+
+  /////
+  //
+  // Autonomous Functions
+  //
+  /////
+
+  /**
+   * Sets the robot to move forward using PID.
+   *
+   * \param target
+   *        target value in inches
+   * \param speed
+   *        0 to 127, max speed during motion
+   * \param slew_on
+   *        ramp up from slew_min to speed over slew_distance.  only use when you're going over about 14"
+   * \param toggle_heading
+   *        toggle for heading correction
+   */
+  void set_drive(double target, int speed, bool slew_on = false, bool toggle_heading = true);
+
+  /*
+
+    Stops movement on the drivetrain
+  */
+  void stop_drive(Drive& chassis);
+
+  /**
+   * Drives the robot forward using a trapezoidal motional profile
+   * 
+   * /param target
+   *  target value in inches
+   * /param endTimeout
+   *  timeout value
+  */
+ void set_proifiled_drive(Drive& chassis, double target, int endTimeout);
+
+  /**
+   * Sets the robot to turn using PID.
+   *
+   * \param target
+   *        target value in degrees
+   * \param speed
+   *        0 to 127, max speed during motion
+   */
+  void set_turn(double target, int speed);
+
+  /**
+   * Turn using only the left or right side.
+   *
+   * \param type
+   *        L_SWING or R_SWING
+   * \param target
+   *        target value in degrees
+   * \param speed
+   *        0 to 127, max speed during motion
+   */
+  void set_swing(e_swing type, double target, int speed);
+
+  /**
+   * Resets all PID targets to 0.
+   */
+  void reset_pid_targets();
+
+  /**
+   * Resets all PID targets to 0.
+   */
+  void set_angle(double angle);
+
+  /**
+   * Lock the code in a while loop until the robot has settled.
+   */
+  void wait_drive();
+
+  /**
+   * Lock the code in a while loop until this position has passed.
+   *
+   * \param target
+   *        when driving, this is inches.  when turning, this is degrees.
+   */
+  void wait_until(double target);
+
+  /**
+   * Autonomous interference detection.  Returns true when interfered, and false when nothing happened.
+   */
+  bool interfered = false;
+
+  /**
+   * Changes max speed during a drive motion.
+   *
+   * \param speed
+   *        new clipped speed
+   */
+  void set_max_speed(int speed);
+
+  /**
+   * Set Either the headingPID, turnPID, forwardPID, backwardPID, activeBrakePID, or swingPID
+   */
+  void set_pid_constants(PID *pid, double p, double i, double d, double p_start_i);
+
+  /**
+   * Sets minimum power for swings when kI and startI are enabled.
+   *
+   * \param min
+   *        new clipped speed
+   */
+  void set_swing_min(int min);
+
+  /**
+   * The minimum power for turns when kI and startI are enabled.
+   *
+   * \param min
+   *        new clipped speed
+   */
+  void set_turn_min(int min);
+
+  /**
+   * Returns minimum power for swings when kI and startI are enabled.
+   */
+  int get_swing_min();
+
+  /**
+   * Returns minimum power for turns when kI and startI are enabled.
+   */
+  int get_turn_min();
+
+  /**
+   * Sets minimum slew speed constants.
+   *
+   * \param fwd
+   *        minimum power for forward drive pd
+   * \param rev
+   *        minimum power for backwards drive pd
+   */
+  void set_slew_min_power(int fwd, int rev);
+
+  /**
+   * Sets minimum slew distance constants.
+   *
+   * \param fw
+   *        minimum distance for forward drive pd
+   * \param bw
+   *        minimum distance for backwards drive pd
+   */
+  void set_slew_distance(int fwd, int rev);
+
+  /**
+   * Set's constants for exit conditions.
+   *
+   * \param &type
+   *        turn_exit, swing_exit, or drive_exit
+   * \param p_small_exit_time
+   *        Sets small_exit_time.  Timer for to exit within smalL_error.
+   * \param p_small_error
+   *        Sets smalL_error. Timer will start when error is within this.
+   * \param p_big_exit_time
+   *        Sets big_exit_time.  Timer for to exit within big_error.
+   * \param p_big_error
+   *        Sets big_error. Timer will start when error is within this.
+   * \param p_velocity_exit_time
+   *        Sets velocity_exit_time.  Timer will start when velocity is 0.
+   */
+  void set_exit_condition(int type, int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout);
+
+  /**
+   * Exit condition for turning.
+   */
+  const int turn_exit = 1;
+
+  /**
+   * Exit condition for swinging.
+   */
+  const int swing_exit = 2;
+
+  /**
+   * Exit condition for driving.
+   */
+  const int drive_exit = 3;
+
+  /**
+   * Returns current tick_per_inch()
+   */
+  double get_tick_per_inch();
+
+  /**
+   * Returns current tick_per_inch()
+   */
+  void modify_curve_with_controller();
+
+  // Slew
+  struct slew_ {
+    int sign = 0;
+    double error = 0;
+    double x_intercept = 0;
+    double y_intercept = 0;
+    double slope = 0;
+    double output = 0;
+    bool enabled = false;
+    double max_speed = 0;
+  };
+
+  slew_ left_slew;
+  slew_ right_slew;
+
+  /**
+   * Initialize slew.
+   *
+   * \param input
+   *        slew_ enum
+   * \param slew_on
+   *        is slew on?
+   * \param max_speed
+   *        target speed during the slew
+   * \param target
+   *        target sensor value
+   * \param current
+   *        current sensor value
+   * \param start
+   *        starting position
+   * \param backwards
+   *        slew direction for constants
+   */
+  void slew_initialize(slew_ &input, bool slew_on, double max_speed, double target, double current, double start, bool backwards);
+
+  /**
+   * Calculate slew.
+   *
+   * \param input
+   *        slew_ enum
+   * \param current
+   *        current sensor value
+   */
+  double slew_calculate(slew_ &input, double current);
+
+ private:  // !Auton
+  bool drive_toggle = true;
+  bool print_toggle = true;
+  int swing_min = 0;
+  int turn_min = 0;
+
+  /**
+   * Heading bool.
+   */
+  bool heading_on = true;
+
+  /**
+   * Active brake kp constant.
+   */
+  double active_brake_kp = 0;
+
+  /**
+   * Tick per inch calculation.
+   */
+  double TICK_PER_REV;
+  double TICK_PER_INCH;
+  double CIRCUMFERENCE;
+
+  double CARTRIDGE;
+  double RATIO;
+  double WHEEL_DIAMETER;
+
+  /**
+   * Max speed for autonomous.
+   */
+  int max_speed;
+
+  /**
+   * Tasks
+   */
+  void drive_pid_task();
+  void swing_pid_task();
+  void turn_pid_task();
+  void ez_auto_task();
+
+  /**
+   * Constants for slew
+   */
+  double SLEW_DISTANCE[2];
+  double SLEW_MIN_POWER[2];
+
+  /**
+   * Starting value for left/right
+   */
+  double l_start = 0;
+  double r_start = 0;
+
+  /**
+   * Enable/disable modifying controller curve with controller.
+   */
+  bool disable_controller = true;  // True enables, false disables.
+
+  /**
+   * Is tank drive running?
+   */
+  bool is_tank;
+
+#define DRIVE_INTEGRATED 1
+#define DRIVE_ADI_ENCODER 2
+#define DRIVE_ROTATION 3
+
+  /**
+   * Is tracking?
+   */
+  int is_tracker = DRIVE_INTEGRATED;
+
+  /**
+   * Save input to sd card
+   */
+  void save_l_curve_sd();
+  void save_r_curve_sd();
+
+  /**
+   * Struct for buttons for increasing/decreasing curve with controller
+   */
+  struct button_ {
+    bool lock = false;
+    bool release_reset = false;
+    int release_timer = 0;
+    int hold_timer = 0;
+    int increase_timer;
+    pros::controller_digital_e_t button;
+  };
+
+  button_ l_increase_;
+  button_ l_decrease_;
+  button_ r_increase_;
+  button_ r_decrease_;
+
+  /**
+   * Function for button presses.
+   */
+  void button_press(button_ *input_name, int button, std::function<void()> changeCurve, std::function<void()> save);
+
+  /**
+   * The left and right curve scalers.
+   */
+  double left_curve_scale;
+  double right_curve_scale;
+
+  /**
+   * Increase and decrease left and right curve scale.
+   */
+  void l_decrease();
+  void l_increase();
+  void r_decrease();
+  void r_increase();
+};
diff --git a/HVN-ON-EARTH/include/ary-lib/util.hpp b/HVN-ON-EARTH/include/ary-lib/util.hpp
new file mode 100644
index 0000000..05c1c4c
--- /dev/null
+++ b/HVN-ON-EARTH/include/ary-lib/util.hpp
@@ -0,0 +1,118 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include <bits/stdc++.h>
+#include <stdio.h>
+#include <string.h>
+#include <vector>
+
+#include "api.h"
+
+/**
+ * Controller.
+ */
+extern pros::Controller master;
+
+namespace ary {
+
+/**
+ * Prints our branding all over your pros terminal
+ */
+void printScr();
+
+/**
+ * Prints to the brain screen in one string.  Splits input between lines with
+ * '\n' or when text longer then 32 characters.
+ *
+ * @param text
+ *        Input string.  Use '\n' for a new line
+ * @param line
+ *        Starting line to print on, defaults to 0
+ */
+void print_to_screen(std::string text, int line = 0);
+
+/////
+//
+// Public Variables
+//
+/////
+
+enum e_curve_type {
+    DEFAULT = 0,
+    LOGARITHMIC = 1,
+    SQRT = 2,
+    SINE = 3,
+};
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_type { SINGLE = 0,
+              SPLIT = 1 };
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_swing { LEFT_SWING = 0,
+               RIGHT_SWING = 1 };
+
+/**
+ * Enum for PID::exit_condition outputs.
+ */
+enum exit_output { RUNNING = 1,
+                   SMALL_EXIT = 2,
+                   BIG_EXIT = 3,
+                   VELOCITY_EXIT = 4,
+                   mA_EXIT = 5,
+                   ERROR_NO_CONSTANTS = 6 };
+
+/**
+ * Enum for split and single stick arcade.
+ */
+enum e_mode { DISABLE = 0,
+              SWING = 1,
+              TURN = 2,
+              DRIVE = 3 };
+
+/**
+ * Outputs string for exit_condition enum.
+ */
+std::string exit_to_string(exit_output input);
+
+namespace util {
+extern bool AUTON_RAN;
+
+/**
+ * Returns 1 if input is positive and -1 if input is negative
+ */
+int signum(double input);
+
+/**
+ * Returns true if the input is < 0
+ */
+bool is_reversed(double input);
+
+/**
+ * Returns input restricted to min-max threshold
+ */
+double clip_num(double input, double max, double min);
+
+
+std::vector<double> trapezoidalMotionProfile(double target, double maxVel, double accel, double decel);
+
+/**
+ * Is the SD card plugged in?
+ */
+const bool IS_SD_CARD = pros::usd::is_installed();
+
+/**
+ * Delay time for tasks
+ */
+const int DELAY_TIME = 10;
+}  // namespace util
+}  // ary
diff --git a/HVN-ON-EARTH/include/asset.h b/HVN-ON-EARTH/include/asset.h
new file mode 100644
index 0000000..682e510
--- /dev/null
+++ b/HVN-ON-EARTH/include/asset.h
@@ -0,0 +1,88 @@
+#pragma once
+
+#ifndef _ASSET_H_
+#define _ASSET_H_
+
+#include "api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef V5_API_H_
+
+typedef struct __attribute__((__packed__)) _v5_image {
+  uint16_t width;
+  uint16_t height;
+  uint32_t *data;
+  uint32_t *p;
+} v5_image;
+
+uint32_t vexImageBmpRead(const uint8_t *ibuf, v5_image *oBuf, uint32_t maxw, uint32_t maxh);
+uint32_t vexImagePngRead(const uint8_t *ibuf, v5_image *oBuf, uint32_t maxw, uint32_t maxh, uint32_t ibuflen);
+void vexDisplayCopyRect(int32_t x1, int32_t y1, int32_t x2, int32_t y2, uint32_t *pSrc, int32_t srcStride);
+
+#endif
+
+typedef struct __attribute__((__packed__)) _asset {
+  uint8_t *buf;
+  size_t size;
+} asset;
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef __cplusplus
+
+#define ASSET(x)                                                                    \
+  extern "C" {                                                                      \
+  extern uint8_t _binary_static_##x##_start[], _binary_static_##x##_size[];         \
+  static asset x = {_binary_static_##x##_start, (size_t)_binary_static_##x##_size}; \
+  }
+
+#else
+
+#define ASSET(x)                                                            \
+  extern uint8_t _binary_static_##x##_start[], _binary_static_##x##_size[]; \
+  static asset x = {_binary_static_##x##_start, (size_t)_binary_static_##x##_size};
+
+#endif
+
+#define DECODE_PNG(var, asset, width, height) \
+  var.data = new uint32_t[width * height];    \
+  vexImagePngRead(asset.buf, &var, width, height, asset.size);
+
+#define DECODE_BMP(var, asset, width, height) \
+  var.data = new uint32_t[width * height];    \
+  vexImageBmpRead(asset.buf, &var, width, height);
+
+
+static void draw_image(int x, int y, v5_image *image) {
+  int x2 = x + image->width - 1;
+  int y2 = y + image->height - 1;
+  vexDisplayCopyRect(x, y, x2, y2, image->data, x2 - x + 1);
+}
+
+static void draw_image(lv_obj_t *canvas, int x, int y, v5_image *image) {
+  int start_x = x;
+  int start_y = y;
+
+  for (int i = 0; i < image->height; i++) {
+    for (int j = 0; j < image->width; j++) {
+      lv_color_t c = lv_color_hex(image->data[i * image->width + j]);
+      lv_color_t d = lv_canvas_get_px(canvas, start_x + j, start_y + i);
+
+      float a_float = c.alpha / 2.55 / 100;
+      float a_m_float = 1 - a_float;
+
+      d.red = d.red * a_m_float + c.red * a_float;
+      d.green = d.green * a_m_float + c.green * a_float;
+      d.blue = d.blue * a_m_float + c.blue * a_float;
+
+      lv_canvas_set_px(canvas, start_x + j, start_y + i, d);
+    }
+  }
+}
+
+#endif // _ASSET_H_
diff --git a/HVN-ON-EARTH/include/autons.hpp b/HVN-ON-EARTH/include/autons.hpp
new file mode 100644
index 0000000..abd163b
--- /dev/null
+++ b/HVN-ON-EARTH/include/autons.hpp
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "ary-lib/drive/drive.hpp"
+
+void near_side();
+void far_side();
+void skills();
+void test_seq();
+void odom_test();
+
+void default_constants();
+void exit_condition_defaults();
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/gif-pros/gifclass.hpp b/HVN-ON-EARTH/include/gif-pros/gifclass.hpp
new file mode 100644
index 0000000..fd45aa7
--- /dev/null
+++ b/HVN-ON-EARTH/include/gif-pros/gifclass.hpp
@@ -0,0 +1,69 @@
+#pragma once
+#include "main.h"
+#include "gifdec.h"
+
+/**
+ * MIT License
+ * Copyright (c) 2019 Theo Lemay
+ * https://github.com/theol0403/gif-pros
+ */
+
+class Gif {
+
+public:
+
+  /**
+   * Construct the Gif class
+   * @param fname  the gif filename on the SD card (prefixed with /usd/)
+   * @param parent the LVGL parent object
+   */
+  Gif(const asset file, lv_obj_t* parent);
+
+  /**
+   * Destructs and cleans the Gif class
+   */
+  ~Gif();
+
+  /**
+   * Pauses the GIF task
+   */
+  void pause();
+
+  /**
+   * Resumes the GIF task
+   */
+  void resume();
+
+  /**
+   * Deletes GIF and frees all allocated memory
+   */
+  void clean();
+
+private:
+
+  gd_GIF* _gif = nullptr; // gif decoder object
+  void* _gifmem = nullptr; // gif file loaded from SD into memory 
+  uint8_t* _buffer = nullptr; // decoder frame buffer
+
+  lv_color_t* _cbuf = nullptr; // canvas buffer
+  lv_obj_t* _canvas = nullptr; // canvas object
+
+  pros::task_t _task = nullptr; // render task
+
+  /**
+   * Cleans and frees all allocated memory
+   */
+  void _cleanup();
+
+  /**
+   * Render cycle, blocks until loop count exceeds gif loop count flag (if any)
+   */
+  void _render();
+
+  /**
+   * Calls _render()
+   * @param arg Gif*
+   */
+  static void _render_task(void* arg);
+
+};
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/gif-pros/gifdec.h b/HVN-ON-EARTH/include/gif-pros/gifdec.h
new file mode 100644
index 0000000..293801e
--- /dev/null
+++ b/HVN-ON-EARTH/include/gif-pros/gifdec.h
@@ -0,0 +1,58 @@
+#ifndef GIFDEC_H
+#define GIFDEC_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BYTES_PER_PIXEL 4
+
+typedef struct gd_Palette {
+    int size;
+    uint8_t colors[0x100 * 3];
+} gd_Palette;
+
+typedef struct gd_GCE {
+    uint16_t delay;
+    uint8_t tindex;
+    uint8_t disposal;
+    int input;
+    int transparency;
+} gd_GCE;
+
+typedef struct gd_GIF {
+    FILE *fp;
+    off_t anim_start;
+    uint16_t width, height;
+    uint16_t depth;
+    uint16_t loop_count;
+    gd_GCE gce;
+    gd_Palette *palette;
+    gd_Palette lct, gct;
+    void (*plain_text)(
+        struct gd_GIF *gif, uint16_t tx, uint16_t ty,
+        uint16_t tw, uint16_t th, uint8_t cw, uint8_t ch,
+        uint8_t fg, uint8_t bg
+    );
+    void (*comment)(struct gd_GIF *gif);
+    void (*application)(struct gd_GIF *gif, char id[8], char auth[3]);
+    uint16_t fx, fy, fw, fh;
+    uint8_t bgindex;
+    uint8_t *canvas, *frame;
+} gd_GIF;
+
+gd_GIF *gd_open_gif(FILE *fp);
+int gd_get_frame(gd_GIF *gif);
+void gd_render_frame(gd_GIF *gif, uint8_t *buffer);
+void gd_rewind(gd_GIF *gif);
+void gd_close_gif(gd_GIF *gif);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* GIFDEC_H */
diff --git a/HVN-ON-EARTH/include/globals.hpp b/HVN-ON-EARTH/include/globals.hpp
new file mode 100644
index 0000000..707ce46
--- /dev/null
+++ b/HVN-ON-EARTH/include/globals.hpp
@@ -0,0 +1,66 @@
+#pragma once
+
+#include "main.h"
+#include "Wings.h"
+
+#define TRACK_WIDTH 11.5
+#define PLACEHOLDER_TC_OFFSET 2.5
+#define WHEEL_SIZE 2.75
+#define DRIVE_RATIO 0.75
+#define DRIVE_RPM 450
+
+namespace globals {
+    extern pros::Controller master;
+
+    extern pros::Motor motor_tlf;
+    extern pros::Motor motor_tlb;
+    extern pros::Motor motor_blf;
+    extern pros::Motor motor_blb;
+    extern pros::Motor motor_trf;
+    extern pros::Motor motor_trb;
+    extern pros::Motor motor_brf;
+    extern pros::Motor motor_brb;
+
+    extern pros::Motor_Group left_drive;
+    extern pros::Motor_Group right_drive;
+
+    extern lemlib::Drivetrain_t dt_odom;
+
+    extern lemlib::OdomSensors_t chassis_sensors;
+
+    extern lemlib::ChassisController_t latController;
+    extern lemlib::ChassisController_t angController;
+
+    extern lemlib::Chassis chassis_odom;
+
+    extern pros::Rotation rot_vert;
+    extern pros::Rotation rot_horiz;
+    extern pros::Rotation enc_theta;
+
+    extern pros::ADIDigitalOut pto_piston;
+
+    extern pros::ADIDigitalOut left_wing_piston;
+    extern pros::ADIDigitalOut right_wing_piston;
+
+    extern pros::ADIDigitalOut intake_piston;
+
+    extern pros::ADIDigitalOut doinker_piston;
+
+    extern Wings wings;
+
+    extern lemlib::TrackingWheel vert_tracking_wheel;
+    extern lemlib::TrackingWheel horiz_tracking_wheel;
+
+    extern pros::Imu inertial_sensor; 
+
+    extern Drive chassis;
+
+    extern pros::Motor& cata_left;
+    extern pros::Motor& cata_right;
+
+    enum e_controlsch {
+        RENU,
+        RIA,
+        CHRIS
+    };
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/lemlib/api.hpp b/HVN-ON-EARTH/include/lemlib/api.hpp
new file mode 100644
index 0000000..26612e4
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/api.hpp
@@ -0,0 +1,17 @@
+/**
+ * @file include/lemlib/api.hpp
+ * @author LemLib Team
+ * @brief LemLib API header file. Include this in your source files to use the library.
+ * @version 0.4.5
+ * @date 2023-01-27
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+
+#include "lemlib/util.hpp"
+#include "lemlib/pid.hpp"
+#include "lemlib/pose.hpp"
+#include "lemlib/chassis/trackingWheel.hpp"
+#include "lemlib/chassis/chassis.hpp"
diff --git a/HVN-ON-EARTH/include/lemlib/chassis/chassis.hpp b/HVN-ON-EARTH/include/lemlib/chassis/chassis.hpp
new file mode 100644
index 0000000..41a630b
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/chassis/chassis.hpp
@@ -0,0 +1,173 @@
+/**
+ * @file include/lemlib/chassis/chassis.hpp
+ * @author LemLib Team
+ * @brief Chassis class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include "pros/motors.hpp"
+#include "pros/imu.hpp"
+#include "lemlib/chassis/trackingWheel.hpp"
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Struct containing all the sensors used for odometry
+ *
+ * The sensors are stored in a struct so that they can be easily passed to the chassis class
+ * The variables are pointers so that they can be set to nullptr if they are not used
+ * Otherwise the chassis class would have to have a constructor for each possible combination of sensors
+ *
+ * @param vertical1 pointer to the first vertical tracking wheel
+ * @param vertical2 pointer to the second vertical tracking wheel
+ * @param horizontal1 pointer to the first horizontal tracking wheel
+ * @param horizontal2 pointer to the second horizontal tracking wheel
+ * @param imu pointer to the IMU
+ */
+typedef struct {
+        TrackingWheel* vertical1;
+        TrackingWheel* vertical2;
+        TrackingWheel* horizontal1;
+        TrackingWheel* horizontal2;
+        pros::Imu* imu;
+} OdomSensors_t;
+
+/**
+ * @brief Struct containing constants for a chassis controller
+ *
+ * The constants are stored in a struct so that they can be easily passed to the chassis class
+ * Set a constant to 0 and it will be ignored
+ *
+ * @param kP proportional constant for the chassis controller
+ * @param kD derivative constant for the chassis controller
+ * @param smallError the error at which the chassis controller will switch to a slower control loop
+ * @param smallErrorTimeout the time the chassis controller will wait before switching to a slower control loop
+ * @param largeError the error at which the chassis controller will switch to a faster control loop
+ * @param largeErrorTimeout the time the chassis controller will wait before switching to a faster control loop
+ * @param slew the maximum acceleration of the chassis controller
+ */
+typedef struct {
+        float kP;
+        float kD;
+        float smallError;
+        float smallErrorTimeout;
+        float largeError;
+        float largeErrorTimeout;
+        float slew;
+} ChassisController_t;
+
+/**
+ * @brief Struct containing constants for a drivetrain
+ *
+ * The constants are stored in a struct so that they can be easily passed to the chassis class
+ * Set a constant to 0 and it will be ignored
+ *
+ * @param leftMotors pointer to the left motors
+ * @param rightMotors pointer to the right motors
+ * @param trackWidth the track width of the robot
+ * @param wheelDiameter the diameter of the wheels (2.75, 3.25, 4, 4.125)
+ * @param rpm the rpm of the wheels
+ */
+typedef struct {
+        pros::Motor_Group* leftMotors;
+        pros::Motor_Group* rightMotors;
+        float trackWidth;
+        float wheelDiameter;
+        float rpm;
+} Drivetrain_t;
+
+/**
+ * @brief Chassis class
+ *
+ */
+class Chassis {
+    public:
+        /**
+         * @brief Construct a new Chassis
+         *
+         * @param drivetrain drivetrain to be used for the chassis
+         * @param lateralSettings settings for the lateral controller
+         * @param angularSettings settings for the angular controller
+         * @param sensors sensors to be used for odometry
+         */
+        Chassis(Drivetrain_t drivetrain, ChassisController_t lateralSettings, ChassisController_t angularSettings,
+                OdomSensors_t sensors);
+        /**
+         * @brief Calibrate the chassis sensors
+         *
+         */
+        void calibrate();
+        /**
+         * @brief Set the pose of the chassis
+         *
+         * @param x new x value
+         * @param y new y value
+         * @param theta new theta value
+         * @param radians true if theta is in radians, false if not. False by default
+         */
+        void setPose(double x, double y, double theta, bool radians = false);
+        /**
+         * @brief Set the pose of the chassis
+         *
+         * @param pose the new pose
+         * @param radians whether pose theta is in radians (true) or not (false). false by default
+         */
+        void setPose(Pose pose, bool radians = false);
+        /**
+         * @brief Get the pose of the chassis
+         *
+         * @param radians whether theta should be in radians (true) or degrees (false). false by default
+         * @return Pose
+         */
+        Pose getPose(bool radians = false);
+        /**
+         * @brief Turn the chassis so it is facing the target point
+         *
+         * The PID logging id is "angularPID"
+         *
+         * @param x x location
+         * @param y y location
+         * @param timeout longest time the robot can spend moving
+         * @param reversed whether the robot should turn in the opposite direction. false by default
+         * @param maxSpeed the maximum speed the robot can turn at. Default is 200
+         * @param log whether the chassis should log the turnTo function. false by default
+         */
+        void turnTo(float x, float y, int timeout, bool reversed = false, float maxSpeed = 127, bool log = false);
+        /**
+         * @brief Move the chassis towards the target point
+         *
+         * The PID logging ids are "angularPID" and "lateralPID"
+         *
+         * @param x x location
+         * @param y y location
+         * @param timeout longest time the robot can spend moving
+         * @param maxSpeed the maximum speed the robot can move at
+         * @param log whether the chassis should log the turnTo function. false by default
+         */
+        void moveTo(float x, float y, int timeout, float maxSpeed = 200, bool log = false);
+        /**
+         * @brief Move the chassis along a path
+         *
+         * @param filePath file path to the path. No need to preface it with /usd/
+         * @param timeout the maximum time the robot can spend moving
+         * @param lookahead the lookahead distance. Units in inches. Larger values will make the robot move faster but
+         * will follow the path less accurately
+         * @param reverse whether the robot should follow the path in reverse. false by default
+         * @param maxSpeed the maximum speed the robot can move at
+         * @param log whether the chassis should log the path on a log file. false by default.
+         */
+        void follow(const char* filePath, int timeout, float lookahead, bool reverse = false, float maxSpeed = 127,
+                    bool log = false);
+    private:
+        ChassisController_t lateralSettings;
+        ChassisController_t angularSettings;
+        Drivetrain_t drivetrain;
+        OdomSensors_t odomSensors;
+};
+} // namespace lemlib
diff --git a/HVN-ON-EARTH/include/lemlib/chassis/odom.hpp b/HVN-ON-EARTH/include/lemlib/chassis/odom.hpp
new file mode 100644
index 0000000..fc0a6c2
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/chassis/odom.hpp
@@ -0,0 +1,50 @@
+/**
+ * @file include/lemlib/chassis/odom.hpp
+ * @author LemLib Team
+ * @brief This is the header file for the odom.cpp file. Its not meant to be used directly, only through the chassis
+ * class
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include "lemlib/chassis/chassis.hpp"
+#include "lemlib/pose.hpp"
+
+namespace lemlib {
+/**
+ * @brief Set the sensors to be used for odometry
+ *
+ * @param sensors the sensors to be used
+ * @param drivetrain drivetrain to be used
+ */
+void setSensors(lemlib::OdomSensors_t sensors, lemlib::Drivetrain_t drivetrain);
+/**
+ * @brief Get the pose of the robot
+ *
+ * @param radians true for theta in radians, false for degrees. False by default
+ * @return Pose
+ */
+Pose getPose(bool radians = false);
+/**
+ * @brief Set the Pose of the robot
+ *
+ * @param pose the new pose
+ * @param radians true if theta is in radians, false if in degrees. False by default
+ */
+void setPose(Pose pose, bool radians = false);
+/**
+ * @brief Update the pose of the robot
+ *
+ */
+void update();
+/**
+ * @brief Initialize the odometry system
+ *
+ */
+void init();
+} // namespace lemlib
diff --git a/HVN-ON-EARTH/include/lemlib/chassis/trackingWheel.hpp b/HVN-ON-EARTH/include/lemlib/chassis/trackingWheel.hpp
new file mode 100644
index 0000000..a212ace
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/chassis/trackingWheel.hpp
@@ -0,0 +1,80 @@
+/**
+ * @file include/lemlib/chassis/trackingWheel.hpp
+ * @author LemLib Team
+ * @brief tracking wheel class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include "pros/motors.hpp"
+#include "pros/adi.hpp"
+#include "pros/rotation.hpp"
+
+namespace lemlib {
+class TrackingWheel {
+    public:
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the optical shaft encoder to use
+         * @param diameter diameter of the tracking wheel in inches
+         * @param distance distance between the tracking wheel and the center of rotation in inches
+         * @param gearRatio gear ratio of the tracking wheel, defaults to 1
+         */
+        TrackingWheel(pros::ADIEncoder* encoder, float diameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param encoder the v5 rotation sensor to use
+         * @param diameter diameter of the tracking wheel in inches
+         * @param distance distance between the tracking wheel and the center of rotation in inches
+         * @param gearRatio gear ratio of the tracking wheel, defaults to 1
+         */
+        TrackingWheel(pros::Rotation* encoder, float diameter, float distance, float gearRatio = 1);
+        /**
+         * @brief Create a new tracking wheel
+         *
+         * @param motors the motor group to use
+         * @param diameter diameter of the drivetrain wheels in inches
+         * @param distance half the track width of the drivetrain in inches
+         * @param rpm theoretical maximum rpm of the drivetrain wheels
+         */
+        TrackingWheel(pros::Motor_Group* motors, float diameter, float distance, float rpm);
+        /**
+         * @brief Reset the tracking wheel position to 0
+         *
+         */
+        void reset();
+        /**
+         * @brief Get the distance traveled by the tracking wheel
+         *
+         * @return float distance traveled in inches
+         */
+        float getDistanceTraveled();
+        /**
+         * @brief Get the offset of the tracking wheel from the center of rotation
+         *
+         * @return float offset in inches
+         */
+        float getOffset();
+        /**
+         * @brief Get the type of tracking wheel
+         *
+         * @return int - 1 if motor group, 0 otherwise
+         */
+        int getType();
+    private:
+        float diameter;
+        float distance;
+        float rpm;
+        pros::ADIEncoder* encoder = nullptr;
+        pros::Rotation* rotation = nullptr;
+        pros::Motor_Group* motors = nullptr;
+        float gearRatio = 1;
+};
+} // namespace lemlib
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/lemlib/logger.hpp b/HVN-ON-EARTH/include/lemlib/logger.hpp
new file mode 100644
index 0000000..e8c2763
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/logger.hpp
@@ -0,0 +1,139 @@
+/**
+ * @file include/lemlib/logger.hpp
+ * @author LemLib Team
+ * @brief A Logger for LemLib.
+ * @version 0.4.5
+ * @date 2023-02-12
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+namespace lemlib {
+
+static bool debug = false;
+static bool verbose = false;
+
+namespace logger {
+
+/**
+ * @brief A level enumeration.
+ *
+ * Debug: Only enabled if lemlib::logger::debug is true
+ * Info: General information
+ * Warn: Warnings, usually not critical/doesn't affect the robot
+ * Error: Errors, usually critical and affects the robot
+ * Fatal: Fatal errors, crashes the program
+ *
+ * @note The log level is inclusive. For example, if the log level is set to
+ */
+enum class Level { DEBUG, INFO, WARN, ERROR, FATAL };
+
+static Level lowestLevel = Level::INFO;
+
+/**
+ * @brief Whether or not to log debug messages.
+ *
+ * @return true if debug is enabled
+ */
+bool isDebug();
+/**
+ * @brief Sets lemlib::debug
+ *
+ * @param debug the new value
+ */
+void setDebug(bool debug);
+
+/**
+ * @brief Whether or not to log info messages.
+ *
+ * If false, only log messages with a level of lemlib::logger::Level::WARN
+ * or higher will be logged
+ */
+bool isVerbose();
+/**
+ * @brief Sets lemlib::verbose
+ *
+ * @param verbose the new value
+ */
+void setVerbose(bool verbose);
+
+/**
+ * @brief The current lowest log level.
+ *
+ * @return the lowest loggable level
+ */
+Level getLowestLevel();
+
+/**
+ * @brief Sets the lowest loggable level
+ *
+ * @param level the new lowest loggable level
+ */
+void setLowestLevel(Level level);
+
+/**
+ * @brief Logs a message with an exception
+ *
+ * @param level the level of the message
+ * @param message the message
+ * @param exception the exception
+ */
+void log(Level level, const char* message, const char* exception);
+/**
+ * @brief Logs a message
+ *
+ * @param level the level of the message
+ * @param message the message
+ */
+void log(Level level, const char* message);
+
+/**
+ * @brief Logs a debug message
+ *
+ * @param message
+ */
+void debug(const char* message);
+/**
+ * @brief Logs an info message
+ *
+ * @param message
+ */
+void info(const char* message);
+/**
+ * @brief Logs a warning message
+ *
+ * @param message
+ */
+void warn(const char* message);
+/**
+ * @brief Logs an error message
+ *
+ * @param message
+ * @param exception
+ */
+void error(const char* message, const char* exception);
+/**
+ * @brief Logs an error message
+ *
+ * @param message
+ */
+void error(const char* message);
+/**
+ * @brief Logs a fatal message
+ *
+ * @param message
+ * @param exception
+ */
+void fatal(const char* message, const char* exception);
+/**
+ * @brief Logs a fatal message
+ *
+ * @param message
+ */
+void fatal(const char* message);
+
+} // namespace logger
+} // namespace lemlib
\ No newline at end of file
diff --git a/HVN-ON-EARTH/include/lemlib/pid.hpp b/HVN-ON-EARTH/include/lemlib/pid.hpp
new file mode 100644
index 0000000..d736374
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/pid.hpp
@@ -0,0 +1,125 @@
+/**
+ * @file include/lemlib/pid.hpp
+ * @author Lemlib Team
+ * @brief FAPID class header
+ * @version 0.4.5
+ * @date 2023-01-15
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+#include <string>
+#include "pros/rtos.hpp"
+
+namespace lemlib {
+/**
+ * @brief Feedforward, Acceleration, Proportional, Integral, Derivative PID controller
+ *
+ * The controller does not loop on its own. It must be called in a loop.
+ * For example: while(!controller.settled) { controller.update(input, output); }
+ *
+ */
+class FAPID {
+    public:
+        /**
+         * @brief Construct a new FAPID
+         *
+         * @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
+         * @param kA acceleration gain, limits the change in output. Set 0 if disabled
+         * @param kP proportional gain, multiplied by error and added to output
+         * @param kI integral gain, multiplied by total error and added to output
+         * @param kD derivative gain, multiplied by change in error and added to output
+         * @param name name of the FAPID. Used for logging
+         */
+        FAPID(float kF, float kA, float kP, float kI, float kD, std::string name);
+        /**
+         * @brief Set gains
+         *
+         * @param kF feedfoward gain, multiplied by target and added to output. Set 0 if disabled
+         * @param kA acceleration gain, limits the change in output. Set 0 if disabled
+         * @param kP proportional gain, multiplied by error and added to output
+         * @param kI integral gain, multiplied by total error and added to output
+         * @param kD derivative gain, multiplied by change in error and added to output
+         */
+        void setGains(float kF, float kA, float kP, float kI, float kD);
+        /**
+         * @brief Set the exit conditions
+         *
+         * @param largeError range where error is considered large
+         * @param smallError range where error is considered small
+         * @param largeTime time in milliseconds t
+         * @param smallTime
+         * @param maxTime
+         */
+        void setExit(float largeError, float smallError, int largeTime, int smallTime, int maxTime);
+        /**
+         * @brief Update the FAPID
+         *
+         * @param target the target value
+         * @param position the current value
+         * @param log whether to check the most recent terminal input for user input. Default is false because logging
+         * multiple PIDs could slow down the program.
+         * @return float - output
+         */
+        float update(float target, float position, bool log = false);
+        /**
+         * @brief Reset the FAPID
+         */
+        void reset();
+        /**
+         * @brief Check if the FAPID has settled
+         *
+         * If the exit conditions have not been set, this function will always return false
+         *
+         * @return true - the FAPID has settled
+         * @return false - the FAPID has not settled
+         */
+        bool settled();
+        /**
+         * @brief initialize the FAPID logging system
+         *
+         * if this function is called, std::cin will be used to interact with the FAPID
+         *
+         * the user can interact with the FAPID through the terminal
+         * the user can access gains and other variables with the following format:
+         * <name>.<variable> to get the value of the variable
+         * <name>.<variable>_<value> to set the value of the variable
+         * for example:
+         * pid.kP_0.5 will set the kP value to 0.5
+         * list of variables thats value can be set:
+         * kF, kA, kP, kI, kD
+         * list of variables that can be accessed:
+         * kF, kA, kP, kI, kD, totalError
+         * list of functions that can be called:
+         * reset()
+         */
+        static void init();
+    private:
+        float kF;
+        float kA;
+        float kP;
+        float kI;
+        float kD;
+
+        float largeError;
+        float smallError;
+        int largeTime = 0;
+        int smallTime = 0;
+        int maxTime = -1; // -1 means no max time set, run forever
+
+        int largeTimeCounter = 0;
+        int smallTimeCounter = 0;
+        int startTime = 0;
+
+        float prevError = 0;
+        float totalError = 0;
+        float prevOutput = 0;
+
+        void log();
+        std::string name;
+        static std::string input;
+        static pros::Task* logTask;
+        static pros::Mutex logMutex;
+};
+} // namespace lemlib
diff --git a/HVN-ON-EARTH/include/lemlib/pose.hpp b/HVN-ON-EARTH/include/lemlib/pose.hpp
new file mode 100644
index 0000000..2e03e9f
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/pose.hpp
@@ -0,0 +1,95 @@
+/**
+ * @file include/lemlib/pose.hpp
+ * @author LemLib Team
+ * @brief Pose class declarations
+ * @version 0.4.5
+ * @date 2023-01-23
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#pragma once
+
+namespace lemlib {
+class Pose {
+    public:
+        /** @brief x value*/
+        float x;
+        /** @brief y value*/
+        float y;
+        /** @brief theta value*/
+        float theta;
+        /**
+         * @brief Create a new pose
+         *
+         * @param x component
+         * @param y component
+         * @param theta heading. Defaults to 0
+         */
+        Pose(float x, float y, float theta = 0);
+        /**
+         * @brief Add a pose to this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        Pose operator+(const Pose& other);
+        /**
+         * @brief Subtract a pose from this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        Pose operator-(const Pose& other);
+        /**
+         * @brief Multiply a pose by this pose
+         *
+         * @param other other pose
+         * @return Pose
+         */
+        float operator*(const Pose& other);
+        /**
+         * @brief Multiply a pose by a float
+         *
+         * @param other float
+         * @return Pose
+         */
+        Pose operator*(const float& other);
+        /**
+         * @brief Divide a pose by a float
+         *
+         * @param other float
+         * @return Pose
+         */
+        Pose operator/(const float& other);
+        /**
+         * @brief Linearly interpolate between two poses
+         *
+         * @param other the other pose
+         * @param t t value
+         * @return Pose
+         */
+        Pose lerp(Pose other, float t);
+        /**
+         * @brief Get the distance between two poses
+         *
+         * @param other the other pose
+         * @return float
+         */
+        float distance(Pose other);
+        /**
+         * @brief Get the angle between two poses
+         *
+         * @param other the other pose
+         * @return float in radians
+         */
+        float angle(Pose other);
+        /**
+         * @brief Rotate a pose by an angle
+         *
+         * @param angle angle in radians
+         * @return Pose
+         */
+        Pose rotate(float angle);
+};
+} // namespace lemlib
diff --git a/HVN-ON-EARTH/include/lemlib/util.hpp b/HVN-ON-EARTH/include/lemlib/util.hpp
new file mode 100644
index 0000000..3547a71
--- /dev/null
+++ b/HVN-ON-EARTH/include/lemlib/util.hpp
@@ -0,0 +1,77 @@
+/**
+ * @file include/lemlib/util.hpp
+ * @author LemLib Team
+ * @brief Utility functions declarations
+ * @version 0.4.5
+ * @date 2023-01-15
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace lemlib {
+/**
+ * @brief Slew rate limiter
+ *
+ * @param target target value
+ * @param current current value
+ * @param maxChange maximum change. No maximum if set to 0
+
+ * @return float - the limited value
+ */
+float slew(float target, float current, float maxChange);
+
+/**
+ * @brief Convert radians to degrees
+ *
+ * @param rad radians
+ * @return float degrees
+ */
+float radToDeg(float rad);
+
+/**
+ * @brief Convert degrees to radians
+ *
+ * @param deg degrees
+ * @return float radians
+ */
+float degToRad(float deg);
+
+/**
+ * @brief Calculate the error between 2 angles. Useful when calculating the error between 2 headings
+ *
+ * @param angle1
+ * @param angle2
+ * @param radians true if angle is in radians, false if not. False by default
+ * @return float wrapped angle
+ */
+float angleError(float angle1, float angle2, bool radians = false);
+
+/**
+ * @brief Return the sign of a number
+ *
+ * @param x the number to get the sign of
+ * @return float - -1 if negative, 1 if positive
+ */
+float sgn(float x);
+
+/**
+ * @brief Return the average of a vector of numbers
+ *
+ * @param values
+ * @return float
+ */
+float avg(std::vector<float> values);
+
+/**
+ * @brief Return the average of a vector of numbers
+ *
+ * @param values
+ * @return double
+ */
+double avg(std::vector<double> values);
+} // namespace lemlib
diff --git a/HVN-ON-EARTH/include/main.h b/HVN-ON-EARTH/include/main.h
index 288056e..6f36cc0 100644
--- a/HVN-ON-EARTH/include/main.h
+++ b/HVN-ON-EARTH/include/main.h
@@ -39,6 +39,16 @@
 /**
  * You should add more #includes here
  */
+#include "ary-lib/api.hpp"
+#include "lemlib/api.hpp"
+
+#include "globals.hpp"
+#include "superstructure.hpp"
+#include "autons.hpp"
+#include "wings.h"
+
+#include "asset.h"
+#include "gif-pros/gifclass.hpp"
 //#include "okapi/api.hpp"
 //#include "pros/api_legacy.h"
 
diff --git a/HVN-ON-EARTH/include/superstructure.hpp b/HVN-ON-EARTH/include/superstructure.hpp
new file mode 100644
index 0000000..a1fed4a
--- /dev/null
+++ b/HVN-ON-EARTH/include/superstructure.hpp
@@ -0,0 +1,68 @@
+#include "main.h"
+
+#define DRIVE_SPEED 110
+#define TURN_SPEED 90
+#define SWING_SPEED 90
+// R1 -> WINGS, L1 -> CATA, L2 -> PTO, R2 -> INTAKE
+// Renu's control preferences
+#define RENU_PTO_TOGGLE DIGITAL_R2
+#define RENU_CATA_CONTROL DIGITAL_R1
+#define RENU_INTAKE_CONTROL DIGITAL_L1
+#define RENU_LEFT_WING_CONTROL DIGITAL_LEFT
+#define RENU_RIGHT_WING_CONTROL DIGITAL_RIGHT
+#define RENU_WING_CONTROL DIGITAL_L2
+
+// Ria's control preferences
+#define RIA_PTO_TOGGLE DIGITAL_LEFT
+#define RIA_CATA_CONTROL DIGITAL_A
+#define RIA_INTAKE_CONTROL DIGITAL_R1
+#define RIA_WINGS_CONTROL DIGITAL_L1
+
+
+namespace superstruct {
+    //configs
+    void chassisInit();
+    void telemetry();
+    void opControlInit();
+    void configureExitConditions();
+    void configureConstants();
+    void autonomousResets();
+    void motorsCoast();
+    void motorsHold();
+    void motorsBrake();
+
+    void disableActiveBrake();
+    
+    // Movement Methods
+    void driveAsync(double dist, bool useHeadingCorrection);
+    void driveSync(double dist, bool useHeadingCorrection);
+    void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist);
+
+    void turnSync(double theta);
+    void turnAsync(double theta);
+
+    void leftSwing(double theta);
+    void rightSwing(double theta);
+    
+    void setDriveScale(double val);
+    void setTurnScale(double val);
+    void setSwingScale(double val);
+
+    //- Structure methods
+    void intakeControl(pros::controller_digital_e_t intakeButton);
+    void togglePto();
+    void runCata();
+    void cataControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton);
+    void wingsControl();
+    void wingsControlSingle(pros::controller_digital_e_t wingControlButton);
+    void wingsControlComplex(pros::controller_analog_e_t leftWingButton, pros::controller_analog_e_t rightWingButton, pros::controller_analog_e_t wingButton);
+
+    //More methods
+    void toggleIntake(bool val);
+    void toggleRemovalMech(bool val);
+
+    void renu_control();
+    void ria_control();
+    void chris_control();
+    
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/project.pros b/HVN-ON-EARTH/project.pros
index 7a3d1f4..f68c539 100644
--- a/HVN-ON-EARTH/project.pros
+++ b/HVN-ON-EARTH/project.pros
@@ -1,316 +1,475 @@
 {
-  "py/object": "pros.conductor.project.Project",
-  "py/state": {
-    "project_name": "HVN-ON-EARTH",
-    "target": "v5",
-    "templates": {
-      "kernel": {
-        "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
-        "metadata": {
-          "cold_addr": "58720256",
-          "cold_output": "bin/cold.package.bin",
-          "hot_addr": "125829120",
-          "hot_output": "bin/hot.package.bin",
-          "origin": "pros-mainline",
-          "output": "bin/monolith.bin"
-        },
-        "name": "kernel",
-        "py/object": "pros.conductor.templates.local_template.LocalTemplate",
-        "supported_kernels": null,
-        "system_files": [
-          "include/display/lv_core/lv_vdb.h",
-          "include/display/lv_core/lv_core.mk",
-          "include/display/lv_misc/lv_math.h",
-          "include/display/lv_objx/lv_tabview.h",
-          "include/display/lv_misc/lv_color.h",
-          "include/display/lv_hal/lv_hal_indev.h",
-          "include/display/lv_fonts/lv_fonts.mk",
-          "include/display/lv_misc/lv_symbol_def.h",
-          "include/display/lv_hal/lv_hal.mk",
-          "include/display/lv_themes/lv_theme_night.h",
-          "include/display/lv_draw/lv_draw_triangle.h",
-          "include/pros/optical.hpp",
-          "include/display/lv_draw/lv_draw_vbasic.h",
-          "include/display/lv_objx/lv_objx_templ.h",
-          "include/display/lv_core/lv_refr.h",
-          "include/pros/link.hpp",
-          "include/display/lv_objx/lv_btnm.h",
-          "include/display/lv_objx/lv_cb.h",
-          "firmware/v5-common.ld",
-          "include/pros/ext_adi.h",
-          "include/pros/rotation.h",
-          "include/display/lv_objx/lv_spinbox.h",
-          "include/display/lv_misc/lv_circ.h",
-          "include/display/lv_misc/lv_mem.h",
-          "include/display/lv_objx/lv_page.h",
-          "include/display/lv_objx/lv_ddlist.h",
-          "include/display/lv_core/lv_group.h",
-          "include/display/lvgl.h",
-          "include/display/lv_objx/lv_chart.h",
-          "include/pros/distance.h",
-          "include/display/lv_objx/lv_list.h",
-          "include/pros/vision.h",
-          "include/pros/misc.hpp",
-          "include/display/lv_draw/lv_draw.h",
-          "include/display/lv_objx/lv_label.h",
-          "include/display/lv_misc/lv_font.h",
-          "include/display/lv_draw/lv_draw_img.h",
-          "include/display/lv_misc/lv_log.h",
-          "include/display/lv_misc/lv_templ.h",
-          "include/pros/llemu.h",
-          "include/display/lv_objx/lv_btn.h",
-          "include/display/lv_fonts/lv_font_builtin.h",
-          "include/display/lv_objx/lv_calendar.h",
-          "firmware/libm.a",
-          "include/display/lv_conf.h",
-          "include/display/lv_objx/lv_sw.h",
-          "include/display/lv_draw/lv_draw_rect.h",
-          "include/display/lv_objx/lv_cont.h",
-          "include/pros/vision.hpp",
-          "include/display/lv_objx/lv_mbox.h",
-          "include/pros/adi.hpp",
-          "include/pros/imu.hpp",
-          "include/display/lv_objx/lv_table.h",
-          "include/pros/screen.hpp",
-          "include/display/lv_draw/lv_draw_label.h",
-          "include/display/lv_misc/lv_txt.h",
-          "include/pros/api_legacy.h",
-          "include/display/lv_objx/lv_lmeter.h",
-          "include/display/lv_themes/lv_theme_templ.h",
-          "include/pros/apix.h",
-          "include/display/lv_draw/lv_draw.mk",
-          "include/display/lv_themes/lv_theme_alien.h",
-          "include/pros/colors.hpp",
-          "include/display/lv_objx/lv_img.h",
-          "firmware/libc.a",
-          "include/display/lv_themes/lv_theme_zen.h",
-          "include/display/lv_themes/lv_theme_material.h",
-          "include/display/lv_draw/lv_draw_arc.h",
-          "include/display/lv_themes/lv_theme_mono.h",
-          "include/display/lv_themes/lv_themes.mk",
-          "include/display/lv_objx/lv_slider.h",
-          "include/pros/serial.h",
-          "include/display/lv_themes/lv_theme.h",
-          "include/display/README.md",
-          "include/display/lv_objx/lv_canvas.h",
-          "include/pros/misc.h",
-          "include/display/lv_misc/lv_fs.h",
-          "include/pros/rtos.h",
-          "include/display/lv_core/lv_indev.h",
-          "include/pros/motors.hpp",
-          "include/display/lv_core/lv_style.h",
-          "include/display/lv_version.h",
-          "include/display/lv_core/lv_lang.h",
-          "include/api.h",
-          "include/display/lv_objx/lv_gauge.h",
-          "include/pros/rtos.hpp",
-          "include/display/lv_hal/lv_hal_disp.h",
-          "include/pros/motors.h",
-          "include/display/lv_objx/lv_led.h",
-          "include/display/lv_draw/lv_draw_rbasic.h",
-          "include/display/lv_objx/lv_kb.h",
-          "include/display/lv_conf_checker.h",
-          "include/display/lv_hal/lv_hal.h",
-          "include/display/lv_draw/lv_draw_line.h",
-          "include/pros/gps.hpp",
-          "include/display/lv_objx/lv_objx.mk",
-          "include/display/lv_objx/lv_win.h",
-          "include/display/lv_core/lv_obj.h",
-          "include/display/lv_objx/lv_arc.h",
-          "include/pros/link.h",
-          "include/display/lv_objx/lv_preload.h",
-          "include/display/lv_misc/lv_area.h",
-          "include/display/lv_misc/lv_ll.h",
-          "include/pros/optical.h",
-          "include/pros/serial.hpp",
-          "include/pros/screen.h",
-          "include/display/lv_themes/lv_theme_nemo.h",
-          "include/pros/llemu.hpp",
-          "firmware/libpros.a",
-          "include/display/lv_misc/lv_gc.h",
-          "include/display/lv_misc/lv_anim.h",
-          "include/display/lv_objx/lv_line.h",
-          "include/pros/distance.hpp",
-          "include/pros/rotation.hpp",
-          "include/pros/error.h",
-          "include/display/lv_objx/lv_tileview.h",
-          "include/pros/gps.h",
-          "include/display/lv_misc/lv_task.h",
-          "include/pros/imu.h",
-          "firmware/v5-hot.ld",
-          "include/display/lv_misc/lv_misc.mk",
-          "include/pros/colors.h",
-          "common.mk",
-          "include/display/lv_objx/lv_roller.h",
-          "include/display/lv_objx/lv_bar.h",
-          "include/display/lv_themes/lv_theme_default.h",
-          "firmware/v5.ld",
-          "include/display/lv_misc/lv_ufs.h",
-          "include/display/lv_hal/lv_hal_tick.h",
-          "include/display/licence.txt",
-          "include/display/lv_objx/lv_ta.h",
-          "include/display/lv_objx/lv_imgbtn.h",
-          "include/pros/adi.h"
-        ],
+    "py/object": "pros.conductor.project.Project",
+    "py/state": {
+        "project_name": "HVN-ON-EARTH",
         "target": "v5",
-        "user_files": [
-          "src/main.cc",
-          ".gitignore",
-          "Makefile",
-          "src/main.cpp",
-          "src/main.c",
-          "include/main.hpp",
-          "include/main.hh",
-          "include/main.h"
-        ],
-        "version": "3.8.0"
-      },
-      "okapilib": {
-        "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\okapilib@4.8.0",
-        "metadata": {
-          "origin": "pros-mainline"
+        "templates": {
+            "LemLib": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\LemLib@0.4.7",
+                "metadata": {
+                    "origin": "local"
+                },
+                "name": "LemLib",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": "^3.8.0",
+                "system_files": [
+                    "firmware\\LemLib.a",
+                    "include\\lemlib\\chassis\\odom.hpp",
+                    "include\\lemlib\\pose.hpp",
+                    "include\\lemlib\\util.hpp",
+                    "include\\lemlib\\chassis\\trackingWheel.hpp",
+                    "include\\lemlib\\pid.hpp",
+                    "include\\lemlib\\api.hpp",
+                    "include\\lemlib\\logger.hpp",
+                    "include\\lemlib\\chassis\\chassis.hpp"
+                ],
+                "target": "v5",
+                "user_files": [],
+                "version": "0.4.7"
+            },
+            "gif-pros-asset": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\gif-pros-asset@1.0.1",
+                "metadata": {
+                    "origin": "local"
+                },
+                "name": "gif-pros-asset",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": "^3.8.0",
+                "system_files": [
+                    "include\\pros\\link.h",
+                    "include\\display\\lv_misc\\lv_ll.h",
+                    "include\\display\\lv_objx\\lv_cb.h",
+                    "include\\display\\lv_objx\\lv_slider.h",
+                    "include\\display\\lv_objx\\lv_ddlist.h",
+                    "include\\display\\lv_themes\\lv_theme_material.h",
+                    "include\\display\\lvgl.h",
+                    "include\\display\\lv_misc\\lv_math.h",
+                    "include\\pros\\llemu.hpp",
+                    "include\\display\\lv_misc\\lv_color.h",
+                    "include\\display\\lv_draw\\lv_draw_rbasic.h",
+                    "include\\pros\\optical.hpp",
+                    "include\\display\\lv_core\\lv_lang.h",
+                    "include\\pros\\llemu.h",
+                    "include\\gif-pros\\gifdec.h",
+                    "include\\api.h",
+                    "include\\pros\\colors.h",
+                    "include\\display\\lv_misc\\lv_area.h",
+                    "include\\pros\\misc.hpp",
+                    "include\\display\\lv_draw\\lv_draw_arc.h",
+                    "include\\pros\\apix.h",
+                    "include\\display\\lv_themes\\lv_theme_nemo.h",
+                    "include\\display\\lv_themes\\lv_theme_zen.h",
+                    "include\\display\\lv_draw\\lv_draw_img.h",
+                    "include\\display\\lv_misc\\lv_anim.h",
+                    "include\\pros\\screen.hpp",
+                    "include\\display\\lv_hal\\lv_hal_indev.h",
+                    "include\\display\\lv_draw\\lv_draw_rect.h",
+                    "include\\display\\lv_misc\\lv_ufs.h",
+                    "include\\display\\lv_objx\\lv_arc.h",
+                    "include\\display\\lv_objx\\lv_calendar.h",
+                    "include\\gif-pros\\gifclass.hpp",
+                    "include\\display\\lv_conf.h",
+                    "include\\pros\\error.h",
+                    "include\\display\\lv_fonts\\lv_font_builtin.h",
+                    "include\\pros\\vision.hpp",
+                    "include\\pros\\ext_adi.h",
+                    "include\\display\\lv_misc\\lv_task.h",
+                    "include\\pros\\rotation.h",
+                    "include\\pros\\colors.hpp",
+                    "include\\display\\lv_version.h",
+                    "include\\display\\lv_objx\\lv_cont.h",
+                    "include\\display\\lv_themes\\lv_theme_mono.h",
+                    "include\\pros\\gps.h",
+                    "include\\pros\\motors.h",
+                    "include\\display\\lv_core\\lv_indev.h",
+                    "include\\display\\lv_objx\\lv_sw.h",
+                    "include\\display\\lv_objx\\lv_ta.h",
+                    "include\\display\\lv_misc\\lv_fs.h",
+                    "include\\display\\lv_hal\\lv_hal.h",
+                    "include\\display\\lv_objx\\lv_img.h",
+                    "include\\display\\lv_objx\\lv_chart.h",
+                    "include\\display\\lv_objx\\lv_btnm.h",
+                    "include\\display\\lv_objx\\lv_spinbox.h",
+                    "include\\pros\\api_legacy.h",
+                    "include\\display\\lv_objx\\lv_btn.h",
+                    "include\\display\\lv_objx\\lv_win.h",
+                    "include\\display\\lv_objx\\lv_imgbtn.h",
+                    "include\\display\\lv_objx\\lv_bar.h",
+                    "include\\display\\lv_objx\\lv_objx_templ.h",
+                    "include\\display\\lv_objx\\lv_line.h",
+                    "include\\pros\\screen.h",
+                    "include\\display\\lv_objx\\lv_label.h",
+                    "include\\pros\\optical.h",
+                    "include\\display\\lv_draw\\lv_draw_label.h",
+                    "include\\pros\\imu.hpp",
+                    "include\\pros\\link.hpp",
+                    "include\\pros\\misc.h",
+                    "include\\display\\lv_themes\\lv_theme_alien.h",
+                    "include\\display\\lv_conf_checker.h",
+                    "include\\display\\lv_core\\lv_obj.h",
+                    "include\\pros\\distance.hpp",
+                    "include\\display\\lv_draw\\lv_draw_triangle.h",
+                    "include\\display\\lv_objx\\lv_preload.h",
+                    "include\\display\\lv_objx\\lv_kb.h",
+                    "include\\display\\lv_core\\lv_vdb.h",
+                    "include\\display\\lv_misc\\lv_gc.h",
+                    "include\\display\\lv_themes\\lv_theme_templ.h",
+                    "include\\display\\lv_themes\\lv_theme_night.h",
+                    "include\\display\\lv_misc\\lv_mem.h",
+                    "include\\display\\lv_objx\\lv_tabview.h",
+                    "firmware\\gif-pros-asset.a",
+                    "include\\display\\lv_misc\\lv_symbol_def.h",
+                    "include\\pros\\serial.hpp",
+                    "include\\display\\lv_misc\\lv_circ.h",
+                    "include\\display\\lv_hal\\lv_hal_tick.h",
+                    "include\\display\\lv_draw\\lv_draw.h",
+                    "include\\pros\\gps.hpp",
+                    "include\\pros\\adi.h",
+                    "include\\pros\\adi.hpp",
+                    "include\\pros\\rotation.hpp",
+                    "include\\display\\lv_objx\\lv_canvas.h",
+                    "include\\pros\\motors.hpp",
+                    "include\\display\\lv_misc\\lv_templ.h",
+                    "include\\display\\lv_misc\\lv_font.h",
+                    "include\\display\\lv_draw\\lv_draw_vbasic.h",
+                    "include\\main.h",
+                    "include\\display\\lv_draw\\lv_draw_line.h",
+                    "include\\asset.h",
+                    "include\\display\\lv_objx\\lv_tileview.h",
+                    "include\\display\\lv_misc\\lv_log.h",
+                    "include\\display\\lv_core\\lv_group.h",
+                    "include\\display\\lv_objx\\lv_led.h",
+                    "include\\display\\lv_core\\lv_style.h",
+                    "include\\pros\\vision.h",
+                    "include\\display\\lv_objx\\lv_mbox.h",
+                    "include\\pros\\rtos.h",
+                    "include\\display\\lv_themes\\lv_theme_default.h",
+                    "include\\pros\\serial.h",
+                    "include\\display\\lv_core\\lv_refr.h",
+                    "include\\pros\\rtos.hpp",
+                    "include\\display\\lv_objx\\lv_gauge.h",
+                    "include\\pros\\distance.h",
+                    "include\\display\\lv_hal\\lv_hal_disp.h",
+                    "include\\display\\lv_objx\\lv_table.h",
+                    "include\\display\\lv_objx\\lv_lmeter.h",
+                    "include\\pros\\imu.h",
+                    "include\\display\\lv_misc\\lv_txt.h",
+                    "include\\display\\lv_objx\\lv_page.h",
+                    "include\\display\\lv_objx\\lv_roller.h",
+                    "include\\display\\lv_themes\\lv_theme.h",
+                    "include\\display\\lv_objx\\lv_list.h"
+                ],
+                "target": "v5",
+                "user_files": [],
+                "version": "1.0.1"
+            },
+            "kernel": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\kernel@3.8.0",
+                "metadata": {
+                    "cold_addr": "58720256",
+                    "cold_output": "bin/cold.package.bin",
+                    "hot_addr": "125829120",
+                    "hot_output": "bin/hot.package.bin",
+                    "origin": "pros-mainline",
+                    "output": "bin/monolith.bin"
+                },
+                "name": "kernel",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": null,
+                "system_files": [
+                    "include/display/lv_core/lv_vdb.h",
+                    "include/display/lv_core/lv_core.mk",
+                    "include/display/lv_misc/lv_math.h",
+                    "include/display/lv_objx/lv_tabview.h",
+                    "include/display/lv_misc/lv_color.h",
+                    "include/display/lv_hal/lv_hal_indev.h",
+                    "include/display/lv_fonts/lv_fonts.mk",
+                    "include/display/lv_misc/lv_symbol_def.h",
+                    "include/display/lv_hal/lv_hal.mk",
+                    "include/display/lv_themes/lv_theme_night.h",
+                    "include/display/lv_draw/lv_draw_triangle.h",
+                    "include/pros/optical.hpp",
+                    "include/display/lv_draw/lv_draw_vbasic.h",
+                    "include/display/lv_objx/lv_objx_templ.h",
+                    "include/display/lv_core/lv_refr.h",
+                    "include/pros/link.hpp",
+                    "include/display/lv_objx/lv_btnm.h",
+                    "include/display/lv_objx/lv_cb.h",
+                    "firmware/v5-common.ld",
+                    "include/pros/ext_adi.h",
+                    "include/pros/rotation.h",
+                    "include/display/lv_objx/lv_spinbox.h",
+                    "include/display/lv_misc/lv_circ.h",
+                    "include/display/lv_misc/lv_mem.h",
+                    "include/display/lv_objx/lv_page.h",
+                    "include/display/lv_objx/lv_ddlist.h",
+                    "include/display/lv_core/lv_group.h",
+                    "include/display/lvgl.h",
+                    "include/display/lv_objx/lv_chart.h",
+                    "include/pros/distance.h",
+                    "include/display/lv_objx/lv_list.h",
+                    "include/pros/vision.h",
+                    "include/pros/misc.hpp",
+                    "include/display/lv_draw/lv_draw.h",
+                    "include/display/lv_objx/lv_label.h",
+                    "include/display/lv_misc/lv_font.h",
+                    "include/display/lv_draw/lv_draw_img.h",
+                    "include/display/lv_misc/lv_log.h",
+                    "include/display/lv_misc/lv_templ.h",
+                    "include/pros/llemu.h",
+                    "include/display/lv_objx/lv_btn.h",
+                    "include/display/lv_fonts/lv_font_builtin.h",
+                    "include/display/lv_objx/lv_calendar.h",
+                    "firmware/libm.a",
+                    "include/display/lv_conf.h",
+                    "include/display/lv_objx/lv_sw.h",
+                    "include/display/lv_draw/lv_draw_rect.h",
+                    "include/display/lv_objx/lv_cont.h",
+                    "include/pros/vision.hpp",
+                    "include/display/lv_objx/lv_mbox.h",
+                    "include/pros/adi.hpp",
+                    "include/pros/imu.hpp",
+                    "include/display/lv_objx/lv_table.h",
+                    "include/pros/screen.hpp",
+                    "include/display/lv_draw/lv_draw_label.h",
+                    "include/display/lv_misc/lv_txt.h",
+                    "include/pros/api_legacy.h",
+                    "include/display/lv_objx/lv_lmeter.h",
+                    "include/display/lv_themes/lv_theme_templ.h",
+                    "include/pros/apix.h",
+                    "include/display/lv_draw/lv_draw.mk",
+                    "include/display/lv_themes/lv_theme_alien.h",
+                    "include/pros/colors.hpp",
+                    "include/display/lv_objx/lv_img.h",
+                    "firmware/libc.a",
+                    "include/display/lv_themes/lv_theme_zen.h",
+                    "include/display/lv_themes/lv_theme_material.h",
+                    "include/display/lv_draw/lv_draw_arc.h",
+                    "include/display/lv_themes/lv_theme_mono.h",
+                    "include/display/lv_themes/lv_themes.mk",
+                    "include/display/lv_objx/lv_slider.h",
+                    "include/pros/serial.h",
+                    "include/display/lv_themes/lv_theme.h",
+                    "include/display/README.md",
+                    "include/display/lv_objx/lv_canvas.h",
+                    "include/pros/misc.h",
+                    "include/display/lv_misc/lv_fs.h",
+                    "include/pros/rtos.h",
+                    "include/display/lv_core/lv_indev.h",
+                    "include/pros/motors.hpp",
+                    "include/display/lv_core/lv_style.h",
+                    "include/display/lv_version.h",
+                    "include/display/lv_core/lv_lang.h",
+                    "include/api.h",
+                    "include/display/lv_objx/lv_gauge.h",
+                    "include/pros/rtos.hpp",
+                    "include/display/lv_hal/lv_hal_disp.h",
+                    "include/pros/motors.h",
+                    "include/display/lv_objx/lv_led.h",
+                    "include/display/lv_draw/lv_draw_rbasic.h",
+                    "include/display/lv_objx/lv_kb.h",
+                    "include/display/lv_conf_checker.h",
+                    "include/display/lv_hal/lv_hal.h",
+                    "include/display/lv_draw/lv_draw_line.h",
+                    "include/pros/gps.hpp",
+                    "include/display/lv_objx/lv_objx.mk",
+                    "include/display/lv_objx/lv_win.h",
+                    "include/display/lv_core/lv_obj.h",
+                    "include/display/lv_objx/lv_arc.h",
+                    "include/pros/link.h",
+                    "include/display/lv_objx/lv_preload.h",
+                    "include/display/lv_misc/lv_area.h",
+                    "include/display/lv_misc/lv_ll.h",
+                    "include/pros/optical.h",
+                    "include/pros/serial.hpp",
+                    "include/pros/screen.h",
+                    "include/display/lv_themes/lv_theme_nemo.h",
+                    "include/pros/llemu.hpp",
+                    "firmware/libpros.a",
+                    "include/display/lv_misc/lv_gc.h",
+                    "include/display/lv_misc/lv_anim.h",
+                    "include/display/lv_objx/lv_line.h",
+                    "include/pros/distance.hpp",
+                    "include/pros/rotation.hpp",
+                    "include/pros/error.h",
+                    "include/display/lv_objx/lv_tileview.h",
+                    "include/pros/gps.h",
+                    "include/display/lv_misc/lv_task.h",
+                    "include/pros/imu.h",
+                    "firmware/v5-hot.ld",
+                    "include/display/lv_misc/lv_misc.mk",
+                    "include/pros/colors.h",
+                    "common.mk",
+                    "include/display/lv_objx/lv_roller.h",
+                    "include/display/lv_objx/lv_bar.h",
+                    "include/display/lv_themes/lv_theme_default.h",
+                    "firmware/v5.ld",
+                    "include/display/lv_misc/lv_ufs.h",
+                    "include/display/lv_hal/lv_hal_tick.h",
+                    "include/display/licence.txt",
+                    "include/display/lv_objx/lv_ta.h",
+                    "include/display/lv_objx/lv_imgbtn.h",
+                    "include/pros/adi.h"
+                ],
+                "target": "v5",
+                "user_files": [
+                    "src/main.cc",
+                    ".gitignore",
+                    "Makefile",
+                    "src/main.cpp",
+                    "src/main.c",
+                    "include/main.hpp",
+                    "include/main.hh",
+                    "include/main.h"
+                ],
+                "version": "3.8.0"
+            },
+            "okapilib": {
+                "location": "C:\\Users\\cjans\\AppData\\Roaming\\PROS\\templates\\okapilib@4.8.0",
+                "metadata": {
+                    "origin": "pros-mainline"
+                },
+                "name": "okapilib",
+                "py/object": "pros.conductor.templates.local_template.LocalTemplate",
+                "supported_kernels": "^3.3.1",
+                "system_files": [
+                    "include/okapi/api/odometry/odomMath.hpp",
+                    "include/okapi/squiggles/physicalmodel/physicalmodel.hpp",
+                    "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp",
+                    "include/okapi/api/chassis/model/hDriveModel.hpp",
+                    "include/okapi/api/chassis/model/readOnlyChassisModel.hpp",
+                    "include/okapi/api/device/button/buttonBase.hpp",
+                    "include/okapi/api/units/QAngularJerk.hpp",
+                    "include/okapi/api/control/iterative/iterativeController.hpp",
+                    "include/okapi/api/control/controllerInput.hpp",
+                    "include/okapi/squiggles/math/quinticpolynomial.hpp",
+                    "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp",
+                    "include/okapi/api/control/async/asyncPositionController.hpp",
+                    "include/okapi/api/filter/passthroughFilter.hpp",
+                    "include/okapi/api/device/rotarysensor/rotarySensor.hpp",
+                    "include/okapi/api/odometry/stateMode.hpp",
+                    "include/okapi/impl/device/controller.hpp",
+                    "include/okapi/api/units/QTime.hpp",
+                    "include/okapi/api/control/util/controllerRunner.hpp",
+                    "include/okapi/api/units/QTorque.hpp",
+                    "include/okapi/impl/device/rotarysensor/IMU.hpp",
+                    "include/okapi/api/control/util/pidTuner.hpp",
+                    "include/okapi/impl/util/timeUtilFactory.hpp",
+                    "include/okapi/api/device/motor/abstractMotor.hpp",
+                    "include/okapi/impl/device/button/controllerButton.hpp",
+                    "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp",
+                    "include/okapi/impl/device/rotarysensor/adiEncoder.hpp",
+                    "include/okapi/api/filter/composableFilter.hpp",
+                    "include/okapi/api/chassis/model/xDriveModel.hpp",
+                    "include/okapi/squiggles/geometry/pose.hpp",
+                    "include/okapi/api/odometry/twoEncoderOdometry.hpp",
+                    "include/okapi/api/util/timeUtil.hpp",
+                    "firmware/squiggles.mk",
+                    "include/okapi/api/control/async/asyncPosPidController.hpp",
+                    "include/okapi/impl/control/util/controllerRunnerFactory.hpp",
+                    "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp",
+                    "include/okapi/api/control/closedLoopController.hpp",
+                    "include/okapi/squiggles/math/utils.hpp",
+                    "include/okapi/api/control/async/asyncVelIntegratedController.hpp",
+                    "include/okapi/api/units/QLength.hpp",
+                    "include/okapi/impl/device/motor/motor.hpp",
+                    "include/okapi/api/odometry/odometry.hpp",
+                    "include/okapi/api/units/RQuantity.hpp",
+                    "include/okapi/api/units/QArea.hpp",
+                    "include/okapi/api/filter/filter.hpp",
+                    "include/okapi/api/device/button/abstractButton.hpp",
+                    "include/okapi/api/units/QPressure.hpp",
+                    "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp",
+                    "include/okapi/api/control/util/flywheelSimulator.hpp",
+                    "include/okapi/api/util/logging.hpp",
+                    "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp",
+                    "include/okapi/api/filter/emaFilter.hpp",
+                    "include/okapi/impl/device/controllerUtil.hpp",
+                    "include/okapi/api/util/mathUtil.hpp",
+                    "include/okapi/api/filter/averageFilter.hpp",
+                    "include/okapi/api/control/iterative/iterativeVelocityController.hpp",
+                    "include/okapi/api/odometry/point.hpp",
+                    "include/okapi/api/control/async/asyncController.hpp",
+                    "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp",
+                    "include/okapi/api/coreProsAPI.hpp",
+                    "include/okapi/api/units/QSpeed.hpp",
+                    "include/okapi/impl/control/util/pidTunerFactory.hpp",
+                    "include/okapi/api/units/QAngle.hpp",
+                    "include/okapi/api/control/async/asyncVelocityController.hpp",
+                    "include/okapi/api/odometry/threeEncoderOdometry.hpp",
+                    "include/okapi/api/filter/velMath.hpp",
+                    "include/okapi/api/control/async/asyncPosIntegratedController.hpp",
+                    "include/okapi/squiggles/constraints.hpp",
+                    "include/okapi/api/filter/filteredControllerInput.hpp",
+                    "include/okapi/api/chassis/controller/chassisControllerPid.hpp",
+                    "include/okapi/api/control/iterative/iterativePositionController.hpp",
+                    "include/okapi/api/control/offsettableControllerInput.hpp",
+                    "include/okapi/squiggles/squiggles.hpp",
+                    "include/okapi/api/units/QAngularSpeed.hpp",
+                    "include/okapi/api/control/iterative/iterativePosPidController.hpp",
+                    "include/okapi/squiggles/physicalmodel/tankmodel.hpp",
+                    "include/okapi/impl/util/rate.hpp",
+                    "include/okapi/api/filter/medianFilter.hpp",
+                    "include/okapi/impl/device/rotarysensor/potentiometer.hpp",
+                    "include/okapi/api/control/iterative/iterativeVelPidController.hpp",
+                    "include/okapi/squiggles/geometry/profilepoint.hpp",
+                    "include/okapi/impl/device/button/adiButton.hpp",
+                    "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp",
+                    "include/okapi/api/filter/demaFilter.hpp",
+                    "include/okapi/api/units/RQuantityName.hpp",
+                    "include/okapi/api/control/util/pathfinderUtil.hpp",
+                    "include/okapi/api/util/abstractTimer.hpp",
+                    "include/okapi/api.hpp",
+                    "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp",
+                    "include/okapi/api/chassis/controller/chassisScales.hpp",
+                    "include/okapi/api/units/QMass.hpp",
+                    "include/okapi/impl/device/rotarysensor/adiGyro.hpp",
+                    "include/okapi/impl/util/configurableTimeUtilFactory.hpp",
+                    "include/okapi/api/control/util/settledUtil.hpp",
+                    "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp",
+                    "include/okapi/impl/util/timer.hpp",
+                    "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp",
+                    "include/okapi/impl/device/motor/motorGroup.hpp",
+                    "include/okapi/api/control/async/asyncWrapper.hpp",
+                    "include/okapi/squiggles/io.hpp",
+                    "include/okapi/api/filter/ekfFilter.hpp",
+                    "include/okapi/api/control/async/asyncMotionProfileController.hpp",
+                    "include/okapi/squiggles/geometry/controlvector.hpp",
+                    "include/okapi/impl/device/distanceSensor.hpp",
+                    "include/okapi/api/control/controllerOutput.hpp",
+                    "include/okapi/api/units/QVolume.hpp",
+                    "include/okapi/squiggles/spline.hpp",
+                    "include/okapi/api/units/QJerk.hpp",
+                    "include/okapi/api/chassis/controller/chassisController.hpp",
+                    "include/okapi/squiggles/physicalmodel/passthroughmodel.hpp",
+                    "include/okapi/api/units/QAngularAcceleration.hpp",
+                    "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp",
+                    "include/okapi/impl/device/adiUltrasonic.hpp",
+                    "include/okapi/impl/device/opticalSensor.hpp",
+                    "include/okapi/api/units/QForce.hpp",
+                    "include/okapi/api/util/supplier.hpp",
+                    "include/okapi/api/chassis/controller/odomChassisController.hpp",
+                    "include/okapi/impl/filter/velMathFactory.hpp",
+                    "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp",
+                    "include/okapi/api/control/async/asyncVelPidController.hpp",
+                    "include/okapi/api/odometry/odomState.hpp",
+                    "include/okapi/api/chassis/model/chassisModel.hpp",
+                    "include/okapi/api/units/QFrequency.hpp",
+                    "include/okapi/impl/device/rotarysensor/rotationSensor.hpp",
+                    "include/okapi/api/util/abstractRate.hpp",
+                    "include/okapi/impl/device/motor/adiMotor.hpp",
+                    "include/okapi/api/units/QAcceleration.hpp",
+                    "include/okapi/api/chassis/model/skidSteerModel.hpp",
+                    "firmware/okapilib.a"
+                ],
+                "target": "v5",
+                "user_files": [],
+                "version": "4.8.0"
+            }
         },
-        "name": "okapilib",
-        "py/object": "pros.conductor.templates.local_template.LocalTemplate",
-        "supported_kernels": "^3.3.1",
-        "system_files": [
-          "include/okapi/api/odometry/odomMath.hpp",
-          "include/okapi/squiggles/physicalmodel/physicalmodel.hpp",
-          "include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp",
-          "include/okapi/api/chassis/model/hDriveModel.hpp",
-          "include/okapi/api/chassis/model/readOnlyChassisModel.hpp",
-          "include/okapi/api/device/button/buttonBase.hpp",
-          "include/okapi/api/units/QAngularJerk.hpp",
-          "include/okapi/api/control/iterative/iterativeController.hpp",
-          "include/okapi/api/control/controllerInput.hpp",
-          "include/okapi/squiggles/math/quinticpolynomial.hpp",
-          "include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp",
-          "include/okapi/api/control/async/asyncPositionController.hpp",
-          "include/okapi/api/filter/passthroughFilter.hpp",
-          "include/okapi/api/device/rotarysensor/rotarySensor.hpp",
-          "include/okapi/api/odometry/stateMode.hpp",
-          "include/okapi/impl/device/controller.hpp",
-          "include/okapi/api/units/QTime.hpp",
-          "include/okapi/api/control/util/controllerRunner.hpp",
-          "include/okapi/api/units/QTorque.hpp",
-          "include/okapi/impl/device/rotarysensor/IMU.hpp",
-          "include/okapi/api/control/util/pidTuner.hpp",
-          "include/okapi/impl/util/timeUtilFactory.hpp",
-          "include/okapi/api/device/motor/abstractMotor.hpp",
-          "include/okapi/impl/device/button/controllerButton.hpp",
-          "include/okapi/impl/device/rotarysensor/integratedEncoder.hpp",
-          "include/okapi/impl/device/rotarysensor/adiEncoder.hpp",
-          "include/okapi/api/filter/composableFilter.hpp",
-          "include/okapi/api/chassis/model/xDriveModel.hpp",
-          "include/okapi/squiggles/geometry/pose.hpp",
-          "include/okapi/api/odometry/twoEncoderOdometry.hpp",
-          "include/okapi/api/util/timeUtil.hpp",
-          "firmware/squiggles.mk",
-          "include/okapi/api/control/async/asyncPosPidController.hpp",
-          "include/okapi/impl/control/util/controllerRunnerFactory.hpp",
-          "include/okapi/impl/chassis/controller/chassisControllerBuilder.hpp",
-          "include/okapi/api/control/closedLoopController.hpp",
-          "include/okapi/squiggles/math/utils.hpp",
-          "include/okapi/api/control/async/asyncVelIntegratedController.hpp",
-          "include/okapi/api/units/QLength.hpp",
-          "include/okapi/impl/device/motor/motor.hpp",
-          "include/okapi/api/odometry/odometry.hpp",
-          "include/okapi/api/units/RQuantity.hpp",
-          "include/okapi/api/units/QArea.hpp",
-          "include/okapi/api/filter/filter.hpp",
-          "include/okapi/api/device/button/abstractButton.hpp",
-          "include/okapi/api/units/QPressure.hpp",
-          "include/okapi/api/control/async/asyncLinearMotionProfileController.hpp",
-          "include/okapi/api/control/util/flywheelSimulator.hpp",
-          "include/okapi/api/util/logging.hpp",
-          "include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp",
-          "include/okapi/api/filter/emaFilter.hpp",
-          "include/okapi/impl/device/controllerUtil.hpp",
-          "include/okapi/api/util/mathUtil.hpp",
-          "include/okapi/api/filter/averageFilter.hpp",
-          "include/okapi/api/control/iterative/iterativeVelocityController.hpp",
-          "include/okapi/api/odometry/point.hpp",
-          "include/okapi/api/control/async/asyncController.hpp",
-          "include/okapi/impl/control/async/asyncMotionProfileControllerBuilder.hpp",
-          "include/okapi/api/coreProsAPI.hpp",
-          "include/okapi/api/units/QSpeed.hpp",
-          "include/okapi/impl/control/util/pidTunerFactory.hpp",
-          "include/okapi/api/units/QAngle.hpp",
-          "include/okapi/api/control/async/asyncVelocityController.hpp",
-          "include/okapi/api/odometry/threeEncoderOdometry.hpp",
-          "include/okapi/api/filter/velMath.hpp",
-          "include/okapi/api/control/async/asyncPosIntegratedController.hpp",
-          "include/okapi/squiggles/constraints.hpp",
-          "include/okapi/api/filter/filteredControllerInput.hpp",
-          "include/okapi/api/chassis/controller/chassisControllerPid.hpp",
-          "include/okapi/api/control/iterative/iterativePositionController.hpp",
-          "include/okapi/api/control/offsettableControllerInput.hpp",
-          "include/okapi/squiggles/squiggles.hpp",
-          "include/okapi/api/units/QAngularSpeed.hpp",
-          "include/okapi/api/control/iterative/iterativePosPidController.hpp",
-          "include/okapi/squiggles/physicalmodel/tankmodel.hpp",
-          "include/okapi/impl/util/rate.hpp",
-          "include/okapi/api/filter/medianFilter.hpp",
-          "include/okapi/impl/device/rotarysensor/potentiometer.hpp",
-          "include/okapi/api/control/iterative/iterativeVelPidController.hpp",
-          "include/okapi/squiggles/geometry/profilepoint.hpp",
-          "include/okapi/impl/device/button/adiButton.hpp",
-          "include/okapi/impl/control/async/asyncVelControllerBuilder.hpp",
-          "include/okapi/api/filter/demaFilter.hpp",
-          "include/okapi/api/units/RQuantityName.hpp",
-          "include/okapi/api/control/util/pathfinderUtil.hpp",
-          "include/okapi/api/util/abstractTimer.hpp",
-          "include/okapi/api.hpp",
-          "include/okapi/impl/control/async/asyncPosControllerBuilder.hpp",
-          "include/okapi/api/chassis/controller/chassisScales.hpp",
-          "include/okapi/api/units/QMass.hpp",
-          "include/okapi/impl/device/rotarysensor/adiGyro.hpp",
-          "include/okapi/impl/util/configurableTimeUtilFactory.hpp",
-          "include/okapi/api/control/util/settledUtil.hpp",
-          "include/okapi/impl/control/iterative/iterativeControllerFactory.hpp",
-          "include/okapi/impl/util/timer.hpp",
-          "include/okapi/api/chassis/controller/defaultOdomChassisController.hpp",
-          "include/okapi/impl/device/motor/motorGroup.hpp",
-          "include/okapi/api/control/async/asyncWrapper.hpp",
-          "include/okapi/squiggles/io.hpp",
-          "include/okapi/api/filter/ekfFilter.hpp",
-          "include/okapi/api/control/async/asyncMotionProfileController.hpp",
-          "include/okapi/squiggles/geometry/controlvector.hpp",
-          "include/okapi/impl/device/distanceSensor.hpp",
-          "include/okapi/api/control/controllerOutput.hpp",
-          "include/okapi/api/units/QVolume.hpp",
-          "include/okapi/squiggles/spline.hpp",
-          "include/okapi/api/units/QJerk.hpp",
-          "include/okapi/api/chassis/controller/chassisController.hpp",
-          "include/okapi/squiggles/physicalmodel/passthroughmodel.hpp",
-          "include/okapi/api/units/QAngularAcceleration.hpp",
-          "include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp",
-          "include/okapi/impl/device/adiUltrasonic.hpp",
-          "include/okapi/impl/device/opticalSensor.hpp",
-          "include/okapi/api/units/QForce.hpp",
-          "include/okapi/api/util/supplier.hpp",
-          "include/okapi/api/chassis/controller/odomChassisController.hpp",
-          "include/okapi/impl/filter/velMathFactory.hpp",
-          "include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp",
-          "include/okapi/api/control/async/asyncVelPidController.hpp",
-          "include/okapi/api/odometry/odomState.hpp",
-          "include/okapi/api/chassis/model/chassisModel.hpp",
-          "include/okapi/api/units/QFrequency.hpp",
-          "include/okapi/impl/device/rotarysensor/rotationSensor.hpp",
-          "include/okapi/api/util/abstractRate.hpp",
-          "include/okapi/impl/device/motor/adiMotor.hpp",
-          "include/okapi/api/units/QAcceleration.hpp",
-          "include/okapi/api/chassis/model/skidSteerModel.hpp",
-          "firmware/okapilib.a"
-        ],
-        "target": "v5",
-        "user_files": [],
-        "version": "4.8.0"
-      }
-    },
-    "upload_options": {
-      "description": "rip relentless </3",
-      "icon": "X"
+        "upload_options": {
+            "description": "rip relentless </3",
+            "icon": "X"
+        }
     }
-  }
 }
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/Wings.cpp b/HVN-ON-EARTH/src/Wings.cpp
new file mode 100644
index 0000000..f491492
--- /dev/null
+++ b/HVN-ON-EARTH/src/Wings.cpp
@@ -0,0 +1,76 @@
+/*
+    Wings.cpp
+    Controls all of the logic for the wings in autonomous and driver control
+*/
+
+#include "main.h"
+#include "Wings.h"
+
+using namespace globals;
+
+Wings::Wings() {
+    /*
+        The state of each wing needs to be manually tracked due to privitization errors.
+        We cannot read the values of the pistons directly, in order to work around this we need to track the state manually.
+    */
+    left_wing_state = 0;
+    right_wing_state = 0;
+    close(); // Force close the wings for safety
+};
+
+void Wings::open() {
+    left_wing_piston.set_value(1);
+    left_wing_state = 1;
+    right_wing_piston.set_value(1);
+    right_wing_state = 1;
+};
+
+void Wings::close() {
+    left_wing_piston.set_value(0);
+    left_wing_state = 0;
+    right_wing_piston.set_value(0);
+    right_wing_state = 0;
+};
+
+void Wings::toggleLeft(int value) {
+    left_wing_piston.set_value(value);  
+    left_wing_state = value; 
+};
+
+void Wings::toggleRight(int value) {
+    right_wing_piston.set_value(value);
+    right_wing_state = value;
+};
+
+/*
+    Opens the wings for a certain amount of time
+    openFor(double duration) -> duration in seconds, converted to milliseconds
+*/
+
+void Wings::openFor(double duration) {
+    open();
+    pros::delay(duration * 1000);
+    close();
+};
+
+/*
+    returns int
+    0 -> Both wings closed
+    1 -> Right wing open
+    2 -> Left wing open
+    3 -> Both wings open
+*/
+
+int Wings::getState() {
+    if (left_wing_state == 0 && right_wing_state == 0) {
+        return 0;
+    } else if (left_wing_state == 0 && right_wing_state == 1) {
+        return 1;
+    } else if (left_wing_state == 1 && right_wing_state == 0) {
+        return 2;
+    } else if (left_wing_state == 1 && right_wing_state == 1) {
+        return 3;
+    } else {
+        return 0;
+    }
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/ary-lib/PID.cpp b/HVN-ON-EARTH/src/ary-lib/PID.cpp
new file mode 100644
index 0000000..6fff97d
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/PID.cpp
@@ -0,0 +1,194 @@
+#include "main.h"
+#include "util.hpp"
+
+using namespace ary;
+using namespace util;
+
+void PID::reset_variables() {
+  output = 0;
+  target = 0;
+  error = 0;
+  lastError = 0;
+  integral = 0;
+  time = 0;
+  prev_time = 0;
+}
+
+PID::PID() {
+  reset_variables();
+  set_constants(0, 0, 0, 0);
+}
+
+PID::Constants PID::get_constants() { return constants; }
+
+// PID constructor with constants
+PID::PID(double p, double i, double d, double start_i, std::string name) {
+  reset_variables();
+  set_constants(p, i, d, start_i);
+  set_name(name);
+}
+
+// Set PID constants
+void PID::set_constants(double p, double i, double d, double p_start_i) {
+  constants.kp = p;
+  constants.ki = i;
+  constants.kd = d;
+  constants.start_i = p_start_i;
+}
+
+// Set exit condition timeouts
+void PID::set_exit_condition(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) {
+  exit.small_exit_time = p_small_exit_time;
+  exit.small_error = p_small_error;
+  exit.big_exit_time = p_big_exit_time;
+  exit.big_error = p_big_error;
+  exit.velocity_exit_time = p_velocity_exit_time;
+  exit.mA_timeout = p_mA_timeout;
+}
+
+void PID::set_target(double input) { target = input; }
+double PID::get_target() { return target; }
+
+double PID::calculate(double dist) {
+  error = target - dist; // Calculate the error
+  derivative = error - lastError; // Calculate the derivative term
+
+  /*
+    If the integral constant is a non-zero value, accumulate the integral term with the calculated error.
+    If the sign of the error is not equal to the sign of the previously recorded error, abandon the integral term.
+  */
+  if (constants.ki != 0) {
+      integral += error;
+
+    if (signum(error) != signum(lastError))
+      integral = 0;
+  }
+
+  output = (error * constants.kp) + (integral * constants.ki) + (derivative * constants.kd); // Combine all of the terms to get an output speed
+
+  lastError = error; // Set the last error to the previously calculated error.
+
+  return output;
+}
+
+void PID::reset_timers() {
+  i = 0;
+  k = 0;
+  j = 0;
+  l = 0;
+  is_mA = false;
+}
+
+void PID::set_name(std::string p_name) {
+  name = p_name;
+  is_name = name == "" ? false : true;
+}
+
+void PID::print_exit(ary::exit_output exit_type) {
+  std::cout << " ";
+  if (is_name)
+    std::cout << name << " PID " << exit_to_string(exit_type) << " Exit.\n";
+  else
+    std::cout << exit_to_string(exit_type) << " Exit.\n";
+}
+
+exit_output PID::exit_condition(bool print) {
+  // If this function is called while all exit constants are 0, print an error
+  if (!(exit.small_error && exit.small_exit_time && exit.big_error && exit.big_exit_time && exit.velocity_exit_time && exit.mA_timeout)) {
+    print_exit(ERROR_NO_CONSTANTS);
+    return ERROR_NO_CONSTANTS;
+  }
+
+  // If the robot gets within the target, make sure it's there for small_timeout amount of time
+  if (exit.small_error != 0) {
+    if (abs(error) < exit.small_error) {
+      j += util::DELAY_TIME;
+      i = 0;  // While this is running, don't run big thresh
+      if (j > exit.small_exit_time) {
+        reset_timers();
+        if (print) print_exit(SMALL_EXIT);
+        return SMALL_EXIT;
+      }
+    } else {
+      j = 0;
+    }
+  }
+
+  // If the robot is close to the target, start a timer.  If the robot doesn't get closer within
+  // a certain amount of time, exit and continue.  This does not run while small_timeout is running
+  if (exit.big_error != 0 && exit.big_exit_time != 0) {  // Check if this condition is enabled
+    if (abs(error) < exit.big_error) {
+      i += util::DELAY_TIME;
+      if (i > exit.big_exit_time) {
+        reset_timers();
+        if (print) print_exit(BIG_EXIT);
+        return BIG_EXIT;
+      }
+    } else {
+      i = 0;
+    }
+  }
+
+  // If the motor velocity is 0,the code will timeout and set interfered to true.
+  if (exit.velocity_exit_time != 0) {  // Check if this condition is enabled
+    if (abs(derivative) <= 0.05) {
+      k += util::DELAY_TIME;
+      if (k > exit.velocity_exit_time) {
+        reset_timers();
+        if (print) print_exit(VELOCITY_EXIT);
+        return VELOCITY_EXIT;
+      }
+    } else {
+      k = 0;
+    }
+  }
+
+  return RUNNING;
+}
+
+exit_output PID::exit_condition(pros::Motor sensor, bool print) {
+  // If the motors are pulling too many mA, the code will timeout and set interfered to true.
+  if (exit.mA_timeout != 0) {  // Check if this condition is enabled
+    if (sensor.is_over_current()) {
+      l += util::DELAY_TIME;
+      if (l > exit.mA_timeout) {
+        reset_timers();
+        if (print) print_exit(mA_EXIT);
+        return mA_EXIT;
+      }
+    } else {
+      l = 0;
+    }
+  }
+
+  return exit_condition(print);
+}
+
+exit_output PID::exit_condition(std::vector<pros::Motor> sensor, bool print) {
+  // If the motors are pulling too many mA, the code will timeout and set interfered to true.
+  if (exit.mA_timeout != 0) {  // Check if this condition is enabled
+    for (auto i : sensor) {
+      // Check if 1 motor is pulling too many mA
+      if (i.is_over_current()) {
+        is_mA = true;
+        break;
+      }
+      // If all of the motors aren't drawing too many mA, keep bool false
+      else {
+        is_mA = false;
+      }
+    }
+    if (is_mA) {
+      l += util::DELAY_TIME;
+      if (l > exit.mA_timeout) {
+        reset_timers();
+        if (print) print_exit(mA_EXIT);
+        return mA_EXIT;
+      }
+    } else {
+      l = 0;
+    }
+  }
+
+  return exit_condition(print);
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/ary-lib/auton.cpp b/HVN-ON-EARTH/src/ary-lib/auton.cpp
new file mode 100644
index 0000000..c0a70f3
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/auton.cpp
@@ -0,0 +1,16 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+
+Auton::Auton() {
+  Name = "";
+  auton_call = nullptr;
+}
+Auton::Auton(std::string name, std::function<void()> callback) {
+  Name = name;
+  auton_call = callback;
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/auton_selector.cpp b/HVN-ON-EARTH/src/ary-lib/auton_selector.cpp
new file mode 100644
index 0000000..653a285
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/auton_selector.cpp
@@ -0,0 +1,38 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+
+AutonSelector::AutonSelector() {
+  auton_count = 0;
+  current_auton_page = 0;
+  Autons = {};
+}
+
+AutonSelector::AutonSelector(std::vector<Auton> autons) {
+  auton_count = autons.size();
+  current_auton_page = 0;
+  Autons = {};
+  Autons.assign(autons.begin(), autons.end());
+}
+
+void AutonSelector::print_selected_auton() {
+  if (auton_count == 0) return;
+  for (int i = 0; i < 8; i++)
+    pros::lcd::clear_line(i);
+  ary::print_to_screen("Page " + std::to_string(current_auton_page + 1) + "\n" + Autons[current_auton_page].Name);
+}
+
+void AutonSelector::call_selected_auton() {
+  if (auton_count == 0) return;
+  Autons[current_auton_page].auton_call();
+}
+
+void AutonSelector::add_autons(std::vector<Auton> autons) {
+  auton_count += autons.size();
+  current_auton_page = 0;
+  Autons.assign(autons.begin(), autons.end());
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/autonselector.cpp b/HVN-ON-EARTH/src/ary-lib/autonselector.cpp
new file mode 100644
index 0000000..ed97a73
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/autonselector.cpp
@@ -0,0 +1,78 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include <filesystem>
+
+#include "main.h"
+
+namespace ary::autonselector {
+AutonSelector auton_selector{};
+
+void page_up() {
+  if (auton_selector.current_auton_page == auton_selector.auton_count - 1)
+    auton_selector.current_auton_page = 0;
+  else
+    auton_selector.current_auton_page++;
+  //update_auto_sd();
+  auton_selector.print_selected_auton();
+}
+
+void page_down() {
+  if (auton_selector.current_auton_page == 0)
+    auton_selector.current_auton_page = auton_selector.auton_count - 1;
+  else
+    auton_selector.current_auton_page--;
+  //update_auto_sd();
+  auton_selector.print_selected_auton();
+}
+
+void initialize() {
+  pros::lcd::initialize();
+  //ary::autonselector::init_auton_selector();
+
+  // Callbacks for auto selector
+  ary::autonselector::auton_selector.print_selected_auton();
+  pros::lcd::register_btn0_cb(ary::autonselector::page_down);
+  pros::lcd::register_btn2_cb(ary::autonselector::page_up);
+}
+
+void shutdown() {
+  pros::lcd::shutdown();
+}
+
+bool turn_off = false;
+
+pros::ADIDigitalIn* left_limit_switch = nullptr;
+pros::ADIDigitalIn* right_limit_switch = nullptr;
+pros::Task limit_switch_task(limitSwitchTask);
+void limit_switch_lcd_initialize(pros::ADIDigitalIn* right_limit, pros::ADIDigitalIn* left_limit) {
+  if (!left_limit && !right_limit) {
+    delete left_limit_switch;
+    delete right_limit_switch;
+    if (pros::millis() <= 100)
+      turn_off = true;
+    return;
+  }
+  turn_off = false;
+  right_limit_switch = right_limit;
+  left_limit_switch = left_limit;
+  limit_switch_task.resume();
+}
+
+void limitSwitchTask() {
+  while (true) {
+    if (right_limit_switch && right_limit_switch->get_new_press())
+      page_up();
+    else if (left_limit_switch && left_limit_switch->get_new_press())
+      page_down();
+
+    if (pros::millis() >= 500 && turn_off)
+      limit_switch_task.suspend();
+
+    pros::delay(50);
+  }
+}
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/drive.cpp b/HVN-ON-EARTH/src/ary-lib/drive/drive.cpp
new file mode 100644
index 0000000..87c0bae
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/drive.cpp
@@ -0,0 +1,341 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "drive.hpp"
+
+#include <list>
+
+#include "main.h"
+#include "pros/llemu.hpp"
+#include "pros/screen.hpp"
+
+using namespace ary;
+
+// Constructor for integrated encoders
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio)
+    : imu(imu_port),
+      left_tracker(-1, -1, false),   // Default value
+      right_tracker(-1, -1, false),  // Default value
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_INTEGRATED;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+// Constructor for tracking wheels plugged into the brain
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio,
+             std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports)
+    : imu(imu_port),
+      left_tracker(abs(left_tracker_ports[0]), abs(left_tracker_ports[1]), util::is_reversed(left_tracker_ports[0])),
+      right_tracker(abs(right_tracker_ports[0]), abs(right_tracker_ports[1]), util::is_reversed(right_tracker_ports[0])),
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ADI_ENCODER;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+// Constructor for tracking wheels plugged into a 3 wire expander
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ticks, double ratio,
+             std::vector<int> left_tracker_ports, std::vector<int> right_tracker_ports, int expander_smart_port)
+    : imu(imu_port),
+      left_tracker({expander_smart_port, abs(left_tracker_ports[0]), abs(left_tracker_ports[1])}, util::is_reversed(left_tracker_ports[0])),
+      right_tracker({expander_smart_port, abs(right_tracker_ports[0]), abs(right_tracker_ports[1])}, util::is_reversed(right_tracker_ports[0])),
+      left_rotation(-1),
+      right_rotation(-1),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ADI_ENCODER;
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = ticks;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+// Constructor for rotation sensors
+Drive::Drive(std::vector<int> left_motor_ports, std::vector<int> right_motor_ports,
+             int imu_port, double wheel_diameter, double ratio,
+             int left_rotation_port, int right_rotation_port)
+    : imu(imu_port),
+      left_tracker(-1, -1, false),   // Default value
+      right_tracker(-1, -1, false),  // Default value
+      left_rotation(abs(left_rotation_port)),
+      right_rotation(abs(right_rotation_port)),
+      ez_auto([this] { this->ez_auto_task(); }) {
+  is_tracker = DRIVE_ROTATION;
+  left_rotation.set_reversed(util::is_reversed(left_rotation_port));
+  right_rotation.set_reversed(util::is_reversed(right_rotation_port));
+
+  // Set ports to a global vector
+  for (auto i : left_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    left_motors.push_back(temp);
+  }
+  for (auto i : right_motor_ports) {
+    pros::Motor temp(abs(i), util::is_reversed(i));
+    right_motors.push_back(temp);
+  }
+
+  // Set constants for tick_per_inch calculation
+  WHEEL_DIAMETER = wheel_diameter;
+  RATIO = ratio;
+  CARTRIDGE = 4096;
+  TICK_PER_INCH = get_tick_per_inch();
+
+  set_defaults();
+}
+
+void Drive::set_defaults() {
+  // PID Constants
+  headingPID = {11, 0, 20, 0};
+  forward_drivePID = {0.45, 0, 5, 0};
+  backward_drivePID = {0.45, 0, 5, 0};
+  turnPID = {5, 0.003, 35, 15};
+  swingPID = {7, 0, 45, 0};
+  leftPID = {0.45, 0, 5, 0};
+  rightPID = {0.45, 0, 5, 0};
+  set_turn_min(30);
+  set_swing_min(30);
+
+  // Slew constants
+  set_slew_min_power(80, 80);
+  set_slew_distance(7, 7);
+
+  // Exit condition constants
+  set_exit_condition(turn_exit, 100, 3, 500, 7, 500, 500);
+  set_exit_condition(swing_exit, 100, 3, 500, 7, 500, 500);
+  set_exit_condition(drive_exit, 80, 50, 300, 150, 500, 500);
+
+  // Modify joystick curve on controller (defaults to disabled)
+  toggle_modify_curve_with_controller(true);
+
+  // Left / Right modify buttons
+  set_left_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
+  set_right_curve_buttons(pros::E_CONTROLLER_DIGITAL_Y, pros::E_CONTROLLER_DIGITAL_A);
+
+  set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
+
+  // Enable auto printing and drive motors moving
+  toggle_auto_drive(true);
+  toggle_auto_print(true);
+
+  // Disables limit switch for auto selector
+  autonselector::limit_switch_lcd_initialize(nullptr, nullptr);
+}
+
+double Drive::get_tick_per_inch() {
+  CIRCUMFERENCE = WHEEL_DIAMETER * 3.141;
+
+  if (is_tracker == DRIVE_ADI_ENCODER || is_tracker == DRIVE_ROTATION)
+    TICK_PER_REV = CARTRIDGE * RATIO;
+  else
+    TICK_PER_REV = (50.0 * (3600.0 / CARTRIDGE)) * RATIO;  // with no cart, the encoder reads 50 counts per rotation
+
+  TICK_PER_INCH = (TICK_PER_REV / CIRCUMFERENCE);
+  return TICK_PER_INCH;
+}
+
+void Drive::set_pid_constants(PID* pid, double p, double i, double d, double p_start_i) {
+  pid->set_constants(p, i, d, p_start_i);
+}
+
+void Drive::set_tank(int left, int right) {
+  if (pros::millis() < 1500) return;
+
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.move_voltage(left * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.move_voltage(right * (12000.0 / 127.0));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+void Drive::set_drive_current_limit(int mA) {
+  if (abs(mA) > 2500) {
+    mA = 2500;
+  }
+  CURRENT_MA = mA;
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+// Motor telemetry
+void Drive::reset_drive_sensor() {
+  left_motors.front().tare_position();
+  right_motors.front().tare_position();
+  if (is_tracker == DRIVE_ADI_ENCODER) {
+    left_tracker.reset();
+    right_tracker.reset();
+    return;
+  } else if (is_tracker == DRIVE_ROTATION) {
+    left_rotation.reset_position();
+    right_rotation.reset_position();
+    return;
+  }
+}
+
+int Drive::right_sensor() {
+  if (is_tracker == DRIVE_ADI_ENCODER)
+    return right_tracker.get_value();
+  else if (is_tracker == DRIVE_ROTATION)
+    return right_rotation.get_position();
+  return right_motors.front().get_position();
+}
+int Drive::right_velocity() { return right_motors.front().get_actual_velocity(); }
+double Drive::right_mA() { return right_motors.front().get_current_draw(); }
+bool Drive::right_over_current() { return right_motors.front().is_over_current(); }
+
+int Drive::left_sensor() {
+  if (is_tracker == DRIVE_ADI_ENCODER)
+    return left_tracker.get_value();
+  else if (is_tracker == DRIVE_ROTATION)
+    return left_rotation.get_position();
+  return left_motors.front().get_position();
+}
+int Drive::left_velocity() { return left_motors.front().get_actual_velocity(); }
+double Drive::left_mA() { return left_motors.front().get_current_draw(); }
+bool Drive::left_over_current() { return left_motors.front().is_over_current(); }
+
+void Drive::reset_gyro(double new_heading) { imu.set_rotation(new_heading); }
+double Drive::get_gyro() { return imu.get_rotation(); }
+
+void Drive::imu_loading_display(int iter) {
+  // If the lcd is already initialized, don't run this function
+  if (pros::lcd::is_initialized()) return;
+
+  // Boarder
+  int boarder = 50;
+
+  // Create the boarder
+  pros::screen::set_pen(COLOR_WHITE);
+  for (int i = 1; i < 3; i++) {
+    pros::screen::draw_rect(boarder + i, boarder + i, 480 - boarder - i, 240 - boarder - i);
+  }
+
+  // While IMU is loading
+  if (iter < 2000) {
+    static int last_x1 = boarder;
+    pros::screen::set_pen(0x00FF6EC7);  // EZ Pink
+    int x1 = (iter * ((480 - (boarder * 2)) / 2000.0)) + boarder;
+    pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder);
+    last_x1 = x1;
+  }
+  // Failsafe time
+  else {
+    static int last_x1 = boarder;
+    pros::screen::set_pen(COLOR_RED);
+    int x1 = ((iter - 2000) * ((480 - (boarder * 2)) / 1000.0)) + boarder;
+    pros::screen::fill_rect(last_x1, boarder, x1, 240 - boarder);
+    last_x1 = x1;
+  }
+}
+
+void Drive::stop_drive(Drive& chassis) {
+  chassis.set_tank(0, 0);
+}
+
+bool Drive::imu_calibrate(bool run_loading_animation) {
+  imu.reset();
+  int iter = 0;
+  while (true) {
+    iter += util::DELAY_TIME;
+
+    if (run_loading_animation) imu_loading_display(iter);
+
+    if (iter >= 2000) {
+      if (!(imu.get_status() & pros::c::E_IMU_STATUS_CALIBRATING)) {
+        break;
+      }
+      if (iter >= 3000) {
+        printf("No IMU plugged in, (took %d ms to realize that)\n", iter);
+        return false;
+      }
+    }
+    pros::delay(util::DELAY_TIME);
+  }
+  master.rumble(".");
+  printf("IMU is done calibrating (took %d ms)\n", iter);
+  return true;
+}
+
+// Brake modes
+void Drive::set_drive_brake(pros::motor_brake_mode_e_t brake_type) {
+  CURRENT_BRAKE = brake_type;
+  for (auto i : left_motors) {
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
+  }
+  for (auto i : right_motors) {
+    if (!pto_check(i)) i.set_brake_mode(brake_type);  // If the motor is in the pto list, don't do anything to the motor.
+  }
+}
+
+void Drive::init_curve_sd() {
+  // do absolutely nothing
+}
+
+void Drive::initialize() {
+  init_curve_sd();
+  imu_calibrate();
+  reset_drive_sensor();
+}
+
+void Drive::toggle_auto_drive(bool toggle) { drive_toggle = toggle; }
+void Drive::toggle_auto_print(bool toggle) { print_toggle = toggle; }
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/exit_conditions.cpp b/HVN-ON-EARTH/src/ary-lib/drive/exit_conditions.cpp
new file mode 100644
index 0000000..71fb3ae
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/exit_conditions.cpp
@@ -0,0 +1,185 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "ary-lib/util.hpp"
+#include "main.h"
+
+using namespace ary;
+
+// Set exit condition timeouts
+void Drive::set_exit_condition(int type, int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout) {
+  if (type == drive_exit) {
+    leftPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+    rightPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  }
+
+  if (type == turn_exit) {
+    turnPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  }
+
+  if (type == swing_exit) {
+    swingPID.set_exit_condition(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
+  }
+}
+
+// User wrapper for exit condition
+void Drive::wait_drive() {
+  // Let the PID run at least 1 iteration
+  pros::delay(util::DELAY_TIME);
+
+  if (mode == DRIVE) {
+    exit_output left_exit = RUNNING;
+    exit_output right_exit = RUNNING;
+    while (left_exit == RUNNING || right_exit == RUNNING) {
+      left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
+      right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Left: " << exit_to_string(left_exit) << " Exit.   Right: " << exit_to_string(right_exit) << " Exit.\n";
+
+    if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+
+  // Turn Exit
+  else if (mode == TURN) {
+    exit_output turn_exit = RUNNING;
+    while (turn_exit == RUNNING) {
+      turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Exit.\n";
+
+    if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+
+  // Swing Exit
+  else if (mode == SWING) {
+    exit_output swing_exit = RUNNING;
+    pros::Motor& sensor = current_swing == ary::LEFT_SWING ? left_motors[0] : right_motors[0];
+    while (swing_exit == RUNNING) {
+      swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
+      pros::delay(util::DELAY_TIME);
+    }
+    if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Exit.\n";
+
+    if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
+      interfered = true;
+    }
+  }
+}
+
+// Function to wait until a certain position is reached.  Wrapper for exit condition.
+void Drive::wait_until(double target) {
+  // If robot is driving...
+  if (mode == DRIVE) {
+    // Calculate error between current and target (target needs to be an in between position)
+    int l_tar = l_start + (target * TICK_PER_INCH);
+    int r_tar = r_start + (target * TICK_PER_INCH);
+    int l_error = l_tar - left_sensor();
+    int r_error = r_tar - right_sensor();
+    int l_sgn = util::signum(l_error);
+    int r_sgn = util::signum(r_error);
+
+    exit_output left_exit = RUNNING;
+    exit_output right_exit = RUNNING;
+
+    while (true) {
+      l_error = l_tar - left_sensor();
+      r_error = r_tar - right_sensor();
+
+      // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+      if (util::signum(l_error) == l_sgn || util::signum(r_error) == r_sgn) {
+        if (left_exit == RUNNING || right_exit == RUNNING) {
+          left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
+          right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
+          pros::delay(util::DELAY_TIME);
+        } else {
+          if (print_toggle) std::cout << "  Left: " << exit_to_string(left_exit) << " Wait Until Exit.   Right: " << exit_to_string(right_exit) << " Wait Until Exit.\n";
+
+          if (left_exit == mA_EXIT || left_exit == VELOCITY_EXIT || right_exit == mA_EXIT || right_exit == VELOCITY_EXIT) {
+            interfered = true;
+          }
+          return;
+        }
+      }
+      // Once we've past target, return
+      else if (util::signum(l_error) != l_sgn || util::signum(r_error) != r_sgn) {
+        if (print_toggle) std::cout << "  Drive Wait Until Exit.\n";
+        return;
+      }
+
+      pros::delay(util::DELAY_TIME);
+    }
+  }
+
+  // If robot is turning or swinging...
+  else if (mode == TURN || mode == SWING) {
+    // Calculate error between current and target (target needs to be an in between position)
+    int g_error = target - get_gyro();
+    int g_sgn = util::signum(g_error);
+
+    exit_output turn_exit = RUNNING;
+    exit_output swing_exit = RUNNING;
+
+    pros::Motor& sensor = current_swing == ary::LEFT_SWING ? left_motors[0] : right_motors[0];
+
+    while (true) {
+      g_error = target - get_gyro();
+
+      // If turning...
+      if (mode == TURN) {
+        // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+        if (util::signum(g_error) == g_sgn) {
+          if (turn_exit == RUNNING) {
+            turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
+            pros::delay(util::DELAY_TIME);
+          } else {
+            if (print_toggle) std::cout << "  Turn: " << exit_to_string(turn_exit) << " Wait Until Exit.\n";
+
+            if (turn_exit == mA_EXIT || turn_exit == VELOCITY_EXIT) {
+              interfered = true;
+            }
+            return;
+          }
+        }
+        // Once we've past target, return
+        else if (util::signum(g_error) != g_sgn) {
+          if (print_toggle) std::cout << "  Turn Wait Until Exit.\n";
+          return;
+        }
+      }
+
+      // If swinging...
+      else {
+        // Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
+        if (util::signum(g_error) == g_sgn) {
+          if (swing_exit == RUNNING) {
+            swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
+            pros::delay(util::DELAY_TIME);
+          } else {
+            if (print_toggle) std::cout << "  Swing: " << exit_to_string(swing_exit) << " Wait Until Exit.\n";
+
+            if (swing_exit == mA_EXIT || swing_exit == VELOCITY_EXIT) {
+              interfered = true;
+            }
+            return;
+          }
+        }
+        // Once we've past target, return
+        else if (util::signum(g_error) != g_sgn) {
+          if (print_toggle) std::cout << "  Swing Wait Until Exit.\n";
+          return;
+        }
+      }
+
+      pros::delay(util::DELAY_TIME);
+    }
+  }
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/pid_tasks.cpp b/HVN-ON-EARTH/src/ary-lib/drive/pid_tasks.cpp
new file mode 100644
index 0000000..8322aa4
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/pid_tasks.cpp
@@ -0,0 +1,98 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+#include "pros/misc.hpp"
+
+using namespace ary;
+
+void Drive::ez_auto_task() {
+  while (true) {
+    // Autonomous PID
+    if (get_mode() == DRIVE)
+      drive_pid_task();
+    else if (get_mode() == TURN)
+      turn_pid_task();
+    else if (get_mode() == SWING)
+      swing_pid_task();
+
+    if (pros::competition::is_autonomous() && !util::AUTON_RAN)
+      util::AUTON_RAN = true;
+    else if (!pros::competition::is_autonomous())
+      set_mode(DISABLE);
+
+    pros::delay(util::DELAY_TIME);
+  }
+}
+
+// Drive PID task
+void Drive::drive_pid_task() {
+  // Compute PID
+  leftPID.calculate(left_sensor());
+  rightPID.calculate(right_sensor());
+  headingPID.calculate(get_gyro());
+
+  // Compute slew
+  double l_slew_out = slew_calculate(left_slew, left_sensor());
+  double r_slew_out = slew_calculate(right_slew, right_sensor());
+
+  // Clip leftPID and rightPID to slew (if slew is disabled, it returns max_speed)
+  double l_drive_out = util::clip_num(leftPID.output, l_slew_out, -l_slew_out);
+  double r_drive_out = util::clip_num(rightPID.output, r_slew_out, -r_slew_out);
+
+  // Toggle heading
+  double gyro_out = heading_on ? headingPID.output : 0;
+
+  // Combine heading and drive
+  double l_out = l_drive_out + gyro_out;
+  double r_out = r_drive_out - gyro_out;
+
+  // Set motors
+  if (drive_toggle)
+    set_tank(l_out, r_out);
+}
+
+// Turn PID task
+void Drive::turn_pid_task() {
+  // Compute PID
+  turnPID.calculate(get_gyro());
+
+  // Clip gyroPID to max speed
+  double gyro_out = util::clip_num(turnPID.output, max_speed, -max_speed);
+
+  // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI
+  if (turnPID.constants.ki != 0 && (fabs(turnPID.get_target()) > turnPID.constants.start_i && fabs(turnPID.error) < turnPID.constants.start_i)) {
+    if (get_turn_min() != 0)
+      gyro_out = util::clip_num(gyro_out, get_turn_min(), -get_turn_min());
+  }
+
+  // Set motors
+  if (drive_toggle)
+    set_tank(gyro_out, -gyro_out);
+}
+
+// Swing PID task
+void Drive::swing_pid_task() {
+  // Compute PID
+  swingPID.calculate(get_gyro());
+
+  // Clip swingPID to max speed
+  double swing_out = util::clip_num(swingPID.output, max_speed, -max_speed);
+
+  // Clip the speed of the turn when the robot is within StartI, only do this when target is larger then StartI
+  if (swingPID.constants.ki != 0 && (fabs(swingPID.get_target()) > swingPID.constants.start_i && fabs(swingPID.error) < swingPID.constants.start_i)) {
+    if (get_swing_min() != 0)
+      swing_out = util::clip_num(swing_out, get_swing_min(), -get_swing_min());
+  }
+
+  if (drive_toggle) {
+    // Check if left or right swing, then set motors accordingly
+    if (current_swing == LEFT_SWING)
+      set_tank(swing_out, 0);
+    else if (current_swing == RIGHT_SWING)
+      set_tank(0, -swing_out);
+  }
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/pto.cpp b/HVN-ON-EARTH/src/ary-lib/drive/pto.cpp
new file mode 100644
index 0000000..6aa24fd
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/pto.cpp
@@ -0,0 +1,54 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include <algorithm>
+#include <vector>
+
+#include "drive.hpp"
+#include "main.h"
+
+bool Drive::pto_check(pros::Motor check_if_pto) {
+  auto does_exist = std::find(pto_active.begin(), pto_active.end(), check_if_pto.get_port());
+  if (does_exist != pto_active.end())
+    return true;  // Motor is in the list
+  return false;   // Motor isn't in the list
+}
+
+void Drive::pto_add(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    // Return if the motor is already in the list
+    if (pto_check(i)) return;
+
+    // Return if the first index was used (this motor is used for velocity)
+    if (i.get_port() == left_motors[0].get_port() || i.get_port() == right_motors[0].get_port()) {
+      printf("You cannot PTO the first index!\n");
+      return;
+    }
+
+    pto_active.push_back(i.get_port());
+  }
+}
+
+void Drive::pto_remove(std::vector<pros::Motor> pto_list) {
+  for (auto i : pto_list) {
+    auto does_exist = std::find(pto_active.begin(), pto_active.end(), i.get_port());
+    // Return if the motor isn't in the list
+    if (does_exist == pto_active.end()) return;
+
+    // Find index of motor
+    int index = std::distance(pto_active.begin(), does_exist);
+    pto_active.erase(pto_active.begin() + index);
+    i.set_brake_mode(CURRENT_BRAKE);  // Set the motor to the brake type of the drive
+    i.set_current_limit(CURRENT_MA);  // Set the motor to the mA of the drive
+  }
+}
+
+void Drive::pto_toggle(std::vector<pros::Motor> pto_list, bool toggle) {
+  if (toggle)
+    pto_add(pto_list);
+  else
+    pto_remove(pto_list);
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/set_pid.cpp b/HVN-ON-EARTH/src/ary-lib/drive/set_pid.cpp
new file mode 100644
index 0000000..cddc0a9
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/set_pid.cpp
@@ -0,0 +1,136 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+
+// Updates max speed
+void Drive::set_max_speed(int speed) {
+  max_speed = util::clip_num(abs(speed), 127, -127);
+}
+
+void Drive::reset_pid_targets() {
+  headingPID.set_target(0);
+  leftPID.set_target(0);
+  rightPID.set_target(0);
+  forward_drivePID.set_target(0);
+  backward_drivePID.set_target(0);
+  turnPID.set_target(0);
+}
+
+void Drive::set_angle(double angle) {
+  headingPID.set_target(angle);
+  reset_gyro(angle);
+}
+
+void Drive::set_mode(e_mode p_mode) {
+  mode = p_mode;
+}
+
+void Drive::set_turn_min(int min) { turn_min = abs(min); }
+int Drive::get_turn_min() { return turn_min; }
+
+void Drive::set_swing_min(int min) { swing_min = abs(min); }
+int Drive::get_swing_min() { return swing_min; }
+
+e_mode Drive::get_mode() { return mode; }
+
+// Set drive PID
+void Drive::set_drive(double target, int speed, bool slew_on, bool toggle_heading) {
+  TICK_PER_INCH = get_tick_per_inch();
+
+  // Print targets
+  if (print_toggle) printf("Drive Started... Target Value: %f (%f ticks)", target, target * TICK_PER_INCH);
+  if (slew_on && print_toggle) printf(" with slew");
+  if (print_toggle) printf("\n");
+
+  // Global setup
+  set_max_speed(speed);
+  heading_on = toggle_heading;
+  bool is_backwards = false;
+  l_start = left_sensor();
+  r_start = right_sensor();
+
+  double l_target_encoder, r_target_encoder;
+
+  // Figure actual target value
+  l_target_encoder = l_start + (target * TICK_PER_INCH);
+  r_target_encoder = r_start + (target * TICK_PER_INCH);
+
+  // Figure out if going forward or backward
+  if (l_target_encoder < l_start && r_target_encoder < r_start) {
+    auto consts = backward_drivePID.get_constants();
+    leftPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i);
+    rightPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i);
+    is_backwards = true;
+  } else {
+    auto consts = forward_drivePID.get_constants();
+    leftPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i);
+    rightPID.set_constants(consts.kp, consts.ki, consts.kd, consts.start_i);
+    is_backwards = false;
+  }
+
+  // Set PID targets
+  leftPID.set_target(l_target_encoder);
+  rightPID.set_target(r_target_encoder);
+
+  // Initialize slew
+  slew_initialize(left_slew, slew_on, max_speed, l_target_encoder, left_sensor(), l_start, is_backwards);
+  slew_initialize(right_slew, slew_on, max_speed, r_target_encoder, right_sensor(), r_start, is_backwards);
+
+  // Run task
+  set_mode(DRIVE);
+}
+
+// Set turn PID
+void Drive::set_turn(double target, int speed) {
+  // Print targets
+  if (print_toggle) printf("Turn Started... Target Value: %f\n", target);
+
+  // Set PID targets
+  turnPID.set_target(target);
+  headingPID.set_target(target);  // Update heading target for next drive motion
+  set_max_speed(speed);
+
+  // Run task
+  set_mode(TURN);
+}
+
+// Set swing PID
+void Drive::set_swing(e_swing type, double target, int speed) {
+  // Print targets
+  if (print_toggle) printf("Swing Started... Target Value: %f\n", target);
+  current_swing = type;
+
+  // Set PID targets
+  swingPID.set_target(target);
+  headingPID.set_target(target);  // Update heading target for next drive motion
+  set_max_speed(speed);
+
+  // Run task
+  set_mode(SWING);
+}
+
+void set_profiled_drive(Drive& chassis, double target, int endTimeout) {
+  //kv: rpm -> voltage
+  //sf: in/ms -> rpm
+  int sign = ary::util::signum(target);
+  target = fabs(target);
+  std::vector<double> profile;
+  // std::cout << "reached 1" << std::endl;
+  if(sign > 0) profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_FWD_ACCEL, LINEAR_FWD_DECEL);
+  else profile = ary::util::trapezoidalMotionProfile(target, LINEAR_MAX_VEL, LINEAR_REV_ACCEL, LINEAR_REV_DECEL);
+  
+  for (int i = 0; i < profile.size(); i++)
+  {
+    chassis.set_tank(profile[i] * VELOCITY_TO_VOLTS_LIN * sign, profile[i] * VELOCITY_TO_VOLTS_LIN * sign);
+    pros::delay(10);
+  }
+  
+  chassis.set_tank(0, 0); // Stop the drive
+  chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
+  pros::delay(endTimeout);
+
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/slew.cpp b/HVN-ON-EARTH/src/ary-lib/drive/slew.cpp
new file mode 100644
index 0000000..23088a0
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/slew.cpp
@@ -0,0 +1,51 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+
+using namespace ary;
+
+// Set minimum power
+void Drive::set_slew_min_power(int fwd, int rev) {
+  SLEW_MIN_POWER[0] = abs(fwd);
+  SLEW_MIN_POWER[1] = abs(rev);
+}
+
+// Set distance to slew for
+void Drive::set_slew_distance(int fwd, int rev) {
+  SLEW_DISTANCE[0] = abs(fwd);
+  SLEW_DISTANCE[1] = abs(rev);
+}
+
+// Initialize slew
+void Drive::slew_initialize(slew_ &input, bool slew_on, double max_speed, double target, double current, double start, bool backwards) {
+  input.enabled = slew_on;
+  input.max_speed = max_speed;
+
+  input.sign = util::signum(target - current);
+  input.x_intercept = start + ((SLEW_DISTANCE[backwards] * input.sign) * TICK_PER_INCH);
+  input.y_intercept = max_speed * input.sign;
+  input.slope = ((input.sign * SLEW_MIN_POWER[backwards]) - input.y_intercept) / (input.x_intercept - 0 - start);  // y2-y1 / x2-x1
+}
+
+// Slew calculation
+double Drive::slew_calculate(slew_ &input, double current) {
+  // Is slew still on?
+  if (input.enabled) {
+    // Error is distance away from completed slew
+    input.error = input.x_intercept - current;
+
+    // When the sign of error flips, slew is completed
+    if (util::signum(input.error) != input.sign)
+      input.enabled = false;
+
+    // Return y=mx+b
+    else if (util::signum(input.error) == input.sign)
+      return ((input.slope * input.error) + input.y_intercept) * input.sign;
+  }
+  // When slew is completed, return max speed
+  return max_speed;
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/drive/user_input.cpp b/HVN-ON-EARTH/src/ary-lib/drive/user_input.cpp
new file mode 100644
index 0000000..43ec08a
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/drive/user_input.cpp
@@ -0,0 +1,247 @@
+#include "main.h"
+
+double JOYSTICK_DRIVE_SCALE = 1;
+double JOYSTICK_TURN_SCALE = 1;
+
+double cata_active_brake_kp = 0.1;
+
+void Drive::save_l_curve_sd() {
+  return;
+}
+
+void Drive::save_r_curve_sd() {
+  return;
+}
+
+// Set curve defaults
+void Drive::set_curve_default(double left, double right) {
+  left_curve_scale = left;
+  right_curve_scale = right;
+
+  save_l_curve_sd();
+  save_r_curve_sd();
+}
+
+void Drive::set_left_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) {
+  l_increase_.button = increase;
+  l_decrease_.button = decrease;
+}
+void Drive::set_right_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) {
+  r_increase_.button = increase;
+  r_decrease_.button = decrease;
+}
+
+void Drive::set_curve_buttons(pros::controller_digital_e_t decrease, pros::controller_digital_e_t increase) {
+  l_increase_.button = increase;
+  l_decrease_.button = decrease;
+  r_increase_.button = increase;
+  r_decrease_.button = decrease;
+}
+
+// Increase / decrease left and right curves
+void Drive::l_increase() { left_curve_scale += 0.1; }
+void Drive::l_decrease() {
+  left_curve_scale -= 0.1;
+  left_curve_scale = left_curve_scale < 0 ? 0 : left_curve_scale;
+}
+void Drive::r_increase() { right_curve_scale += 0.1; }
+void Drive::r_decrease() {
+  right_curve_scale -= 0.1;
+  right_curve_scale = right_curve_scale < 0 ? 0 : right_curve_scale;
+}
+
+// Button press logic for increase/decrease curves
+void Drive::button_press(button_* input_name, int button, std::function<void()> change_curve, std::function<void()> save) {
+  // If button is pressed, increase the curve and set toggles.
+  if (button && !input_name->lock) {
+    change_curve();
+    input_name->lock = true;
+    input_name->release_reset = true;
+  }
+
+  // If the button is still held, check if it's held for 500ms.
+  // Then, increase the curve every 100ms by 0.1
+  else if (button && input_name->lock) {
+    input_name->hold_timer += ary::util::DELAY_TIME;
+    if (input_name->hold_timer > 500.0) {
+      input_name->increase_timer += ary::util::DELAY_TIME;
+      if (input_name->increase_timer > 100.0) {
+        change_curve();
+        input_name->increase_timer = 0;
+      }
+    }
+  }
+
+  // When button is released for 250ms, save the new curve value to the SD card
+  else if (!button) {
+    input_name->lock = false;
+    input_name->hold_timer = 0;
+
+    if (input_name->release_reset) {
+      input_name->release_timer += ary::util::DELAY_TIME;
+      if (input_name->release_timer > 250.0) {
+        save();
+        input_name->release_timer = 0;
+        input_name->release_reset = false;
+      }
+    }
+  }
+}
+
+// Toggle modifying curves with controller
+void Drive::toggle_modify_curve_with_controller(bool toggle) { disable_controller = toggle; }
+
+// Modify curves with button presses and display them to contrller
+void Drive::modify_curve_with_controller() {
+  if (!disable_controller) return;  // True enables, false disables.
+
+  button_press(&l_increase_, master.get_digital(l_increase_.button), ([this] { this->l_increase(); }), ([this] { this->save_l_curve_sd(); }));
+  button_press(&l_decrease_, master.get_digital(l_decrease_.button), ([this] { this->l_decrease(); }), ([this] { this->save_l_curve_sd(); }));
+  if (!is_tank) {
+    button_press(&r_increase_, master.get_digital(r_increase_.button), ([this] { this->r_increase(); }), ([this] { this->save_r_curve_sd(); }));
+    button_press(&r_decrease_, master.get_digital(r_decrease_.button), ([this] { this->r_decrease(); }), ([this] { this->save_r_curve_sd(); }));
+  }
+
+  auto sr = std::to_string(right_curve_scale);
+  auto sl = std::to_string(left_curve_scale);
+  if (!is_tank)
+    master.set_text(2, 0, sl + "   " + sr);
+  else
+    master.set_text(2, 0, sl);
+}
+
+// Left curve function
+double Drive::left_curve_function(double x) {
+  if (left_curve_scale != 0) {
+    // if (CURVE_TYPE)
+    return (powf(2.718, -(left_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(left_curve_scale / 10)))) * x;
+    // else
+    // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x;
+  }
+  return x;
+}
+
+// Right curve fnuction
+double Drive::right_curve_function(double x) {
+  if (right_curve_scale != 0) {
+    // if (CURVE_TYPE)
+    return (powf(2.718, -(right_curve_scale / 10)) + powf(2.718, (fabs(x) - 127) / 10) * (1 - powf(2.718, -(right_curve_scale / 10)))) * x;
+    // else
+    // return powf(2.718, ((abs(x)-127)*RIGHT_CURVE_SCALE)/100)*x;
+  }
+  return x;
+}
+
+// Set active brake constant
+void Drive::set_active_brake(double kp) { active_brake_kp = kp; }
+
+void Drive::set_cata_active_brake(double kp) { active_brake_kp = kp; }
+
+double Drive::get_cata_active_brake() { return cata_active_brake_kp; }
+
+// Set joystick threshold
+void Drive::set_joystick_threshold(int threshold) { JOYSTICK_THRESHOLD = abs(threshold); }
+
+void Drive::reset_drive_sensors_opcontrol() {
+  if (util::AUTON_RAN) {
+    reset_drive_sensor();
+    util::AUTON_RAN = false;
+  }
+}
+
+void Drive::set_joystick_drivescale(double scaleval) { JOYSTICK_DRIVE_SCALE = scaleval; }
+
+void Drive::set_joystick_turnscale(double scaleval) { JOYSTICK_TURN_SCALE = scaleval; }
+
+void Drive::joy_thresh_opcontrol(int l_stick, int r_stick) {
+  // Threshold if joysticks don't come back to perfect 0
+  if (abs(l_stick) > JOYSTICK_THRESHOLD || abs(r_stick) > JOYSTICK_THRESHOLD) {
+    set_tank(l_stick, r_stick);
+    if (active_brake_kp != 0) reset_drive_sensor();
+  }
+  // When joys are released, run active brake (P) on drive
+  else {
+    set_tank((0 - left_sensor()) * active_brake_kp, (0 - right_sensor()) * active_brake_kp);
+  }
+}
+
+// Tank control
+void Drive::tank_control() {
+  is_tank = true;
+  reset_drive_sensors_opcontrol();
+
+  // Toggle for controller curve
+  modify_curve_with_controller();
+
+  // Put the joysticks through the curve function
+  int l_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
+  int r_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_Y));
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(l_stick, r_stick);
+}
+
+// Arcade standarddouble JOYSTICK_DRIVE_SCALE = 1;
+
+// double JOYSTICK_DRIVE_SCALE = 1;
+// double JOYSTICK_TURN_SCALE = 0.6;
+void Drive::arcade_standard(e_type stick_type, e_curve_type curve_type) {
+  is_tank = false;
+  reset_drive_sensors_opcontrol();
+
+  // Toggle for controller curve
+  modify_curve_with_controller();
+
+  double fwd_stick, turn_stick;
+  // Check arcade type (split vs single, normal vs flipped)
+  if (stick_type == SPLIT) {
+    // Put the joysticks through the curve function
+    fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
+    turn_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_X));
+  } else if (stick_type == SINGLE) {
+    // Put the joysticks through the curve function
+    if (curve_type == DEFAULT) {
+      fwd_stick = left_curve_function(master.get_analog(ANALOG_LEFT_Y));
+      turn_stick = right_curve_function(master.get_analog(ANALOG_LEFT_X));
+    } else if (curve_type == LOGARITHMIC) {
+      //fwd_stick = (master.get_analog(ANALOG_LEFT_Y) >= 1) ? 10
+    } else if (curve_type == SQRT) {
+
+    } else if (curve_type == SINE) {
+
+    } else {
+      std::cout << "bruh it no work" << std::endl;
+    }
+    
+
+    fwd_stick *= JOYSTICK_DRIVE_SCALE;
+    turn_stick *= JOYSTICK_TURN_SCALE;
+  }
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(fwd_stick + turn_stick, fwd_stick - turn_stick);
+}
+
+// Arcade control flipped
+void Drive::arcade_flipped(e_type stick_type) {
+  is_tank = false;
+  reset_drive_sensors_opcontrol();
+
+  // Toggle for controller curve
+  modify_curve_with_controller();
+
+  int turn_stick, fwd_stick;
+  // Check arcade type (split vs single, normal vs flipped)
+  if (stick_type == SPLIT) {
+    // Put the joysticks through the curve function
+    fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y));
+    turn_stick = left_curve_function(master.get_analog(ANALOG_LEFT_X));
+  } else if (stick_type == SINGLE) {
+    // Put the joysticks through the curve function
+    fwd_stick = right_curve_function(master.get_analog(ANALOG_RIGHT_Y));
+    turn_stick = left_curve_function(master.get_analog(ANALOG_RIGHT_X));
+  }
+
+  // Set robot to l_stick and r_stick, check joystick threshold, set active brake
+  joy_thresh_opcontrol(fwd_stick + turn_stick, fwd_stick - turn_stick);
+}
diff --git a/HVN-ON-EARTH/src/ary-lib/util.cpp b/HVN-ON-EARTH/src/ary-lib/util.cpp
new file mode 100644
index 0000000..812184a
--- /dev/null
+++ b/HVN-ON-EARTH/src/ary-lib/util.cpp
@@ -0,0 +1,184 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "main.h"
+
+pros::Controller master(pros::E_CONTROLLER_MASTER);
+
+namespace ary {
+int mode = DISABLE;
+
+void printScr() {
+  std::cout << R"(
+
+
+    _____ ______   _____                    _       _
+   |  ___|___  /  |_   _|                  | |     | |
+   | |__    / /_____| | ___ _ __ ___  _ __ | | __ _| |_ ___
+   |  __|  / /______| |/ _ \ '_ ` _ \| '_ \| |/ _` | __/ _ \
+   | |___./ /___    | |  __/ | | | | | |_) | | (_| | ||  __/
+   \____/\_____/    \_/\___|_| |_| |_| .__/|_|\__,_|\__\___|
+                                     | |
+                                     |_|
+)" << '\n';
+
+  printf("Version: 2.1.1\n");
+}
+std::string get_last_word(std::string text) {
+  std::string word = "";
+  for (int i = text.length() - 1; i >= 0; i--) {
+    if (text[i] != ' ') {
+      word += text[i];
+    } else {
+      std::reverse(word.begin(), word.end());
+      return word;
+    }
+  }
+  std::reverse(word.begin(), word.end());
+  return word;
+}
+std::string get_rest_of_the_word(std::string text, int position) {
+  std::string word = "";
+  for (int i = position; i < text.length(); i++) {
+    if (text[i] != ' ' && text[i] != '\n') {
+      word += text[i];
+    } else {
+      return word;
+    }
+  }
+  return word;
+}
+//All iance\n\nWE WIN THESE!!!!! 
+void print_to_screen(std::string text, int line) {
+  int CurrAutoLine = line;
+  std::vector<string> texts = {};
+  std::string temp = "";
+
+  for (int i = 0; i < text.length(); i++) {
+    if (text[i] != '\n' && temp.length() + 1 > 32) {
+      auto last_word = get_last_word(temp);
+      if (last_word == temp) {
+        texts.push_back(temp);
+        temp = text[i];
+      } else {
+        int size = last_word.length(); 
+
+        auto rest_of_word = get_rest_of_the_word(text, i); 
+        temp.erase(temp.length() - size, size);
+        texts.push_back(temp);
+        last_word += rest_of_word;
+        i += rest_of_word.length();
+        temp = last_word;
+        if (i >= text.length() - 1) {
+          texts.push_back(temp);
+          break;
+        }
+        
+      }
+    }
+    if (i >= text.length() - 1) {
+      temp += text[i];
+      texts.push_back(temp);
+      temp = "";
+      break;
+    } else if (text[i] == '\n') {
+      texts.push_back(temp);
+      temp = "";
+    } else {
+      temp += text[i];
+    }
+  }
+  for (auto i : texts) {
+    if (CurrAutoLine > 7) {
+      pros::lcd::clear();
+      pros::lcd::set_text(line, "Out of Bounds. Print Line is too far down");
+      return;
+    }
+    pros::lcd::clear_line(CurrAutoLine);
+    pros::lcd::set_text(CurrAutoLine, i);
+    CurrAutoLine++;
+  }
+}
+
+std::string exit_to_string(exit_output input) {
+  switch ((int)input) {
+    case RUNNING:
+      return "Running";
+    case SMALL_EXIT:
+      return "Small";
+    case BIG_EXIT:
+      return "Big";
+    case VELOCITY_EXIT:
+      return "Velocity";
+    case mA_EXIT:
+      return "mA";
+    case ERROR_NO_CONSTANTS:
+      return "Error: Exit condition constants not set!";
+    default:
+      return "Error: Out of bounds!";
+  }
+
+  return "Error: Out of bounds!";
+}
+namespace util {
+bool AUTON_RAN = true;
+
+bool is_reversed(double input) {
+  if (input < 0) return true;
+  return false;
+}
+
+int signum(double input) {
+  if (input > 0)
+    return 1;
+  else if (input < 0)
+    return -1;
+  return 0;
+}
+
+double clip_num(double input, double max, double min) {
+  if (input > max)
+    return max;
+  else if (input < min)
+    return min;
+  return input;
+}
+
+std::vector<double> trapezoidalMotionProfile(double dist, double maxVel, double accel, double decel) {
+  double max = std::min(std::sqrt((2 * accel * decel * dist) / accel + decel), maxVel);
+  double accelTime = max / accel;
+  double decelTime = max / decel;
+  double coastDist = (dist / max) - (max / (2 * accel)) - (max / (2 * decel));
+  double coastTime = coastDist / max;
+  double totalTime = accelTime + decelTime + coastTime;
+  double vel = 0;
+  double diff;
+  std::vector<double> profile;
+
+  for (int i = 0; i < std::ceil(totalTime); i++)
+  {
+    if (i < std::floor(accelTime))
+    {
+      profile.push_back(vel);
+      vel += accel;
+    }
+
+    else if (i < coastTime + accelTime)
+    {
+      profile.push_back(max);
+    }
+
+    else
+    {
+      profile.push_back(vel);
+      vel -= decel;
+    }
+  }
+  return profile;
+}
+
+}  // namespace util
+}  // namespace ary
diff --git a/HVN-ON-EARTH/src/autons.cpp b/HVN-ON-EARTH/src/autons.cpp
new file mode 100644
index 0000000..d856624
--- /dev/null
+++ b/HVN-ON-EARTH/src/autons.cpp
@@ -0,0 +1,83 @@
+#include "main.h"
+#include <stdlib.h>
+
+using namespace globals;
+using namespace superstruct;
+
+/*
+  TODO:
+    - AUTONS
+      - SameBarQual
+      - OppBarQual
+      - SameBarElims
+      - OppBar Elims
+*/
+
+/*
+  chassis.wait_drive() -> Waits for the drivetrain to complete whatever movement is happening
+  chassis.wait_until(double measurement) -> Waits for the drivetrain to reach a certain measurement before performing the next task.
+*/
+
+void near_side() {
+  // Score the triball preload
+  toggleIntake(true);
+  driveSync(100, true);
+  turnSync(90);
+
+  toggleIntake(false);
+  pros::delay(50);
+  driveSync(22, true);
+  driveSync(-22, true);
+
+  turnSync(-120);
+  driveSync(48.5, true);
+  pros::delay(150);
+  toggleIntake(true);
+  pros::delay(100);
+
+  driveSync(-35, true);
+  turnSync(90);
+
+  pros::delay(100);
+  toggleIntake(false);
+  driveSync(55, true);
+  driveSync(-20, true);
+
+  //driveSync(100, true);
+  
+}
+
+void far_side() {
+  toggleIntake(true);
+  driveSync(100, true);
+  turnSync(-90);
+  
+  toggleIntake(false);
+  pros::delay(250);
+  driveSync(31, true);
+  driveSync(-30, true);
+
+  turnSync(-160);
+  driveSync(125, true);
+  pros::delay(1000);
+  driveSync(-35, true);
+
+  turnSync(55);
+  driveSync(70, true);
+
+}
+
+void skills() {
+  
+}
+
+void test_seq() {
+  driveSync(10, true);
+  turnSync(90);
+  turnSync(90);
+}
+
+void odom_test() {
+  chassis_odom.moveTo(10, 10, 1000);
+  chassis_odom.turnTo(15, 15, 1000);
+};
diff --git a/HVN-ON-EARTH/src/globals.cpp b/HVN-ON-EARTH/src/globals.cpp
new file mode 100644
index 0000000..fe77ca9
--- /dev/null
+++ b/HVN-ON-EARTH/src/globals.cpp
@@ -0,0 +1,95 @@
+/*
+    globals.cpp
+    
+    Contains all devices and electronics used for the robot
+    Holds other mechanicsm classes.
+*/
+
+#include "globals.hpp"
+
+using namespace ary;
+namespace globals {
+    // Chassis
+    pros::Controller master(CONTROLLER_MASTER);
+
+    /*
+        {-2, -6, 12, 5}, -> left
+        {-16, 1, 4, -3}, -> right
+    */
+
+    pros::Motor motor_tlf(12, MOTOR_GEARSET_06, false);
+    pros::Motor motor_blb(6, MOTOR_GEARSET_06, true);
+    pros::Motor motor_blf(2, MOTOR_GEARSET_06, true);
+    pros::Motor motor_tlb(5, MOTOR_GEARSET_06, false);
+    pros::Motor motor_trf(16, MOTOR_GEARSET_06, true);
+    pros::Motor motor_trb(3, MOTOR_GEARSET_06, true);
+    pros::Motor motor_brf(1, MOTOR_GEARSET_06, false);
+    pros::Motor motor_brb(4, MOTOR_GEARSET_06, false);
+
+    pros::Motor_Group left_drive({ motor_tlf, motor_tlb, motor_blf, motor_blb });
+    pros::Motor_Group right_drive({ motor_trf, motor_trb, motor_brf, motor_brb });
+
+    // Electronics / Pneumatics / Sensors
+    pros::ADIDigitalOut pto_piston('A');
+
+    pros::ADIDigitalOut left_wing_piston('E');
+    pros::ADIDigitalOut right_wing_piston('F');
+
+    pros::ADIDigitalOut intake_piston('D');
+
+    //pros::ADIDigitalOut doinker_piston('E');
+
+    Wings wings;
+
+    pros::Imu inertial_sensor(18);
+
+    lemlib::Drivetrain_t dt_odom {
+        &left_drive,
+        &right_drive,
+        TRACK_WIDTH,
+        WHEEL_SIZE,
+        DRIVE_RPM
+    };
+
+    lemlib::OdomSensors_t chassis_sensors {
+        nullptr,
+        nullptr,
+        nullptr,
+        nullptr,
+        &inertial_sensor
+    };
+
+    lemlib::ChassisController_t latController {
+        0.55, // kP
+        5, // kD
+        1, // smallErrorRange
+        100, // smallErrorTimeout
+        3, // largeErrorRange
+        500, // largeErrorTimeout
+        5 // slew rate
+    };
+
+    lemlib::ChassisController_t angController {
+        6.5, // kP
+        35, // kD
+        1, // smallErrorRange
+        100, // smallErrorTimeout
+        3, // largeErrorRange
+        500, // largeErrorTimeout
+        40 // slew rate
+    };
+
+    Drive chassis(
+        {-2, -6, 12, 5},
+        {-16, 1, 4, -3},
+        18,
+        WHEEL_SIZE,
+        600,
+        DRIVE_RATIO
+    );
+    
+    lemlib::Chassis chassis_odom(dt_odom, latController, angController, chassis_sensors);
+
+    pros::Motor& cata_left = chassis.left_motors[3];
+    pros::Motor& cata_right = chassis.right_motors[3];
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/src/main.cpp b/HVN-ON-EARTH/src/main.cpp
index c2fbbf5..5652a04 100644
--- a/HVN-ON-EARTH/src/main.cpp
+++ b/HVN-ON-EARTH/src/main.cpp
@@ -1,20 +1,17 @@
+/*
+	main.cpp
+
+	The entry point for the bot
+*/
+
 #include "main.h"
 
-/**
- * A callback function for LLEMU's center button.
- *
- * When this callback is fired, it will toggle line 2 of the LCD text between
- * "I was pressed!" and nothing.
- */
-void on_center_button() {
-	static bool pressed = false;
-	pressed = !pressed;
-	if (pressed) {
-		pros::lcd::set_text(2, "I was pressed!");
-	} else {
-		pros::lcd::clear_line(2);
-	}
-}
+ASSET(chip_gif)
+
+using namespace globals;
+using namespace superstruct;
+
+e_controlsch currentuser = RENU;
 
 /**
  * Runs initialization code. This occurs as soon as the program is started.
@@ -23,71 +20,72 @@ void on_center_button() {
  * to keep execution time for this mode under a few seconds.
  */
 void initialize() {
-	pros::lcd::initialize();
-	pros::lcd::set_text(1, "Hello PROS User!");
+	ary::printScr();
+  
+	pros::delay(500); // Give time for legacy ports configure
 
-	pros::lcd::register_btn1_cb(on_center_button);
+	/*
+		Construct the chassis and it's constants
+		chassisInit() -> Adjusts drive curves, active brake constants, among other things 
+		configureConstants() -> Sets PID constants on the chassis
+		configureExitConditions() -> Sets default values for PID Exit conditions
+	*/
+	
+	chassisInit();
+	configureConstants();
+	configureExitConditions();
+
+	
+	/*
+		Add autonomous paths to the auto selection
+	*/
+	ary::autonselector::auton_selector.add_autons({
+		Auton("Near side (close to alliance goal) \n\nTo run when near alliance goal", near_side),
+		Auton("Far side (far from alliance goal) \n\nTo run when far from alliance goal", far_side),
+		Auton("Skills \n\nPEAK!!!", skills),
+		Auton("TESTING TURNS", test_seq),
+		Auton("Testing Odometry \n\nBaltimore behavior", odom_test)
+	});
+
+	motorsCoast(); // Allow the motors to coast initially
+
+	chassis.initialize();
+	ary::autonselector::initialize();
+	//pros::lcd::register_btn1_cb(centerButtonCallback);
+	pros::Task telemetryTask(telemetry);
 }
 
-/**
- * Runs while the robot is in the disabled state of Field Management System or
- * the VEX Competition Switch, following either autonomous or opcontrol. When
- * the robot is enabled, this task will exit.
- */
 void disabled() {}
-
-/**
- * Runs after initialize(), and before autonomous when connected to the Field
- * Management System or the VEX Competition Switch. This is intended for
- * competition-specific initialization routines, such as an autonomous selector
- * on the LCD.
- *
- * This task will exit when the robot is enabled and autonomous or opcontrol
- * starts.
- */
 void competition_initialize() {}
 
-/**
- * Runs the user autonomous code. This function will be started in its own task
- * with the default priority and stack size whenever the robot is enabled via
- * the Field Management System or the VEX Competition Switch in the autonomous
- * mode. Alternatively, this function may be called in initialize or opcontrol
- * for non-competition testing purposes.
- *
- * If the robot is disabled or communications is lost, the autonomous task
- * will be stopped. Re-enabling the robot will restart the task, not re-start it
- * from where it left off.
- */
-void autonomous() {}
+void autonomous() {
+	autonomousResets();
+	ary::autonselector::auton_selector.call_selected_auton();
+}
 
-/**
- * Runs the operator control code. This function will be started in its own task
- * with the default priority and stack size whenever the robot is enabled via
- * the Field Management System or the VEX Competition Switch in the operator
- * control mode.
- *
- * If no competition control is connected, this function will run immediately
- * following initialize().
- *
- * If the robot is disabled or communications is lost, the
- * operator control task will be stopped. Re-enabling the robot will restart the
- * task, not resume it from where it left off.
- */
 void opcontrol() {
-	pros::Controller master(pros::E_CONTROLLER_MASTER);
-	pros::Motor left_mtr(1);
-	pros::Motor right_mtr(2);
+	Gif gif(chip_gif, lv_scr_act());	
+	opControlInit(); // Configure the chassis for driver control
 
-	while (true) {
-		pros::lcd::print(0, "%d %d %d", (pros::lcd::read_buttons() & LCD_BTN_LEFT) >> 2,
-		                 (pros::lcd::read_buttons() & LCD_BTN_CENTER) >> 1,
-		                 (pros::lcd::read_buttons() & LCD_BTN_RIGHT) >> 0);
-		int left = master.get_analog(ANALOG_LEFT_Y);
-		int right = master.get_analog(ANALOG_RIGHT_Y);
+	while (true) {	
+		/*
+			Handle controls for whoever is driving the bot at the moment, each available user a respective set of controls'
+			Available Options:
+			RENU, RIA, CHRIS
+		*/
+		if (currentuser == RENU) {
+			chassis.tank_control();
+			renu_control();
+		} else if (currentuser == RIA) {
+			chassis.tank_control();
+			renu_control();
+		} else if (currentuser == CHRIS) {
+			chassis.arcade_standard(ary::SINGLE, ary::DEFAULT);
+			chris_control();
+		} else {
+			renu_control(); 
+		}
 
-		left_mtr = left;
-		right_mtr = right;
-
-		pros::delay(20);
+ 		pros::delay(ary::util::DELAY_TIME);
 	}
 }
diff --git a/HVN-ON-EARTH/src/superstructure.cpp b/HVN-ON-EARTH/src/superstructure.cpp
new file mode 100644
index 0000000..05f1a37
--- /dev/null
+++ b/HVN-ON-EARTH/src/superstructure.cpp
@@ -0,0 +1,253 @@
+#include "superstructure.hpp"
+
+using namespace ary;
+using namespace globals;
+
+bool ptoEnabled = false;
+bool wingsOpen = false;
+bool intakeEngaged = false;
+
+/*
+  SCALE SPEEDS: Determines what percentage speeds of autonomous movements should move at
+    speedScale -> The scale of how fast the drivetrain goes forward and backwards
+    turnScale -> The scale of how fast the drivetrain turns
+    swingScale -> The scale of fast one side of the chassis moves
+*/
+
+double speedScale = 1.0;
+double turnScale = 1.0;
+double swingScale = 1.0;
+namespace superstruct {
+  void chassisInit() {
+    /*
+      When the robot first starts up we want to do a couple things:
+      - Adjust the drivetrain curve bottons so it does not interfere with any of the driver controls.
+      - Enable the joystick curve
+      - Enable active break on the drive
+        - Active break is a P controller applied to the drivetrain in order to help it maintain it's position, resisting external forces.
+        - 
+    */
+
+    chassis.set_curve_buttons(pros::E_CONTROLLER_DIGITAL_LEFT, pros::E_CONTROLLER_DIGITAL_RIGHT);
+    chassis.toggle_modify_curve_with_controller(true);
+    chassis.set_active_brake(0.1);
+    chassis.set_curve_default(0.375, 0.375);
+
+    /* Adjust the adjust the factor by which the drive velocity is adjusted */ 
+    chassis.set_joystick_drivescale(1.0);
+    chassis.set_joystick_turnscale(1.0); 
+
+    chassis_odom.calibrate();
+    chassis_odom.setPose(0, 0, 0);
+  }
+
+  void telemetry() {
+    while (true) {
+      lemlib::Pose bot_pose = chassis_odom.getPose();
+      pros::lcd::print(0, "x: %f", bot_pose.x); // print the x position
+        pros::lcd::print(1, "y: %f", bot_pose.y); // print the y position
+        pros::lcd::print(2, "heading: %f", bot_pose.theta); // print the heading
+        pros::delay(10);
+    }
+  }
+
+  void opControlInit() {
+    motorsCoast();
+	  disableActiveBrake();
+  }
+
+  // Adjust exit conditions to allow for quick movements
+  void configureExitConditions() {
+    chassis.set_exit_condition(chassis.turn_exit, 50, 2, 220, 3, 500, 500);
+    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
+    chassis.set_exit_condition(chassis.drive_exit, 40, 80, 300, 150, 500, 500);
+  }
+
+  // Adjust PID constants for accurate movements
+  void configureConstants() {
+    chassis.set_slew_min_power(80, 80);
+    chassis.set_slew_distance(7, 7);
+    chassis.set_pid_constants(&chassis.headingPID, 16, 0, 32, 0);
+    chassis.set_pid_constants(&chassis.forward_drivePID, 0.55, 0, 5, 0);
+    chassis.set_pid_constants(&chassis.backward_drivePID, 0.55, 0, 5, 0);
+    chassis.set_pid_constants(&chassis.turnPID, 6.5, 0.003, 35, 15);
+    chassis.set_pid_constants(&chassis.swingPID, 8.5, 0, 50, 0);
+  }
+
+  // Prepare the bot for the autonomous period of a match
+  void autonomousResets() {
+    chassis.reset_pid_targets();
+  	chassis.reset_gyro();
+  	chassis.reset_drive_sensor();
+    configureConstants();
+    configureExitConditions();
+    motorsBrake();
+    
+  }
+
+  void motorsCoast() {
+    chassis.set_drive_brake(MOTOR_BRAKE_COAST);
+  }
+
+  void motorsHold() {
+    chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
+  }
+
+  void motorsBrake() {
+    chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
+  }
+
+  // The chassis will not apply a constant voltage to prevent it from being moved
+  void disableActiveBrake() {
+    chassis.set_active_brake(0.0);
+  }
+
+
+  // Drives forward, runs next commands WITHOUT waiting for the drive to complete 
+  void driveAsync(double dist, bool useHeadingCorrection) {
+    //chassis.set_mode(ary::DRIVE);
+    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
+  }
+
+  // Drives forward, runs next commands AFTER waiting for the drive to complete 
+  void driveSync(double dist, bool useHeadingCorrection) {
+    //chassis.set_mode(ary::DRIVE);
+    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
+    chassis.wait_drive();
+  }
+
+  // Drives forward, runs next commands AFTER reaching a certain measurement/error along the path
+  void driveWithMD(double dist, bool useHeadingCorrection, double waitUntilDist) {
+    //chassis.set_mode(ary::DRIVE);
+    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
+    chassis.wait_until(waitUntilDist);
+  }
+
+  // Turns the chassis, runs other commands after it has run.
+  void turnSync(double theta) {
+    //chassis.set_mode(ary::TURN);
+    chassis.set_turn(theta, TURN_SPEED * turnScale);
+    chassis.wait_drive();
+  }
+
+  // Turns the chassis, runs other commands immediately after call
+  void turnAsync(double theta) {
+    //chassis.set_mode(ary::TURN);
+    chassis.set_turn(theta, TURN_SPEED * turnScale);
+  }
+
+  // Moves only the right side of the chassis so it can make a left turn
+  void leftSwing(double theta) {
+    //chassis.set_mode(SWING);
+    chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingScale);
+  }
+
+  // Moves only the left side of the chassis so it can make a right turn
+  void rightSwing(double theta) {
+    //chassis.set_mode(SWING);
+    chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingScale);
+  }
+
+  /*
+    Each of the scale values must be clamed between 0.1 - 1 (10% to 100%) to avoid saturation of motors.
+  */
+
+  void setDriveScale(double val) {
+    speedScale = std::clamp(val, 0.1, 1.0);
+  }
+
+  void setTurnScale(double val) {
+    turnScale = std::clamp(val, 0.1, 1.0);
+  }
+
+  void setSwingScale(double val) {
+    swingScale = std::clamp(val, 0.1, 1.0); // 
+  }
+
+  // Structure methods
+  void intakeControl(pros::controller_digital_e_t intakeButton) {
+    if (globals::master.get_digital_new_press(intakeButton)) {
+      if (intakeEngaged == false) {
+        intake_piston.set_value(1);
+        intakeEngaged = true;
+      } else if (intakeEngaged == true) {
+        intake_piston.set_value(0);
+        intakeEngaged = false;
+      }
+    }
+  }
+
+  void togglePto(bool toggle) {
+    ptoEnabled = toggle;
+    chassis.pto_toggle({cata_left, cata_right}, toggle); // Configure the listed PTO motors to whatever value toggle is.
+    pto_piston.set_value(toggle);
+
+    if (toggle) {
+      cata_left.set_brake_mode(MOTOR_BRAKE_COAST);
+      cata_right.set_brake_mode(MOTOR_BRAKE_COAST);
+    }
+  }
+
+  void runCata(double inpt) {
+    if (!ptoEnabled) return;
+    cata_left = inpt;
+    cata_right = inpt;
+  }
+
+  int lock = 0;
+  void cataControl(pros::controller_digital_e_t ptoToggleButton, pros::controller_digital_e_t cataRunButton) {
+    if (globals::master.get_digital(ptoToggleButton) && lock == 0) { // If the PTO button has been pressed and the PTO is not engaged
+      togglePto(!ptoEnabled); // Toggle the PTO so that cataput is useable
+      lock = 1;
+    } else if(!globals::master.get_digital(ptoToggleButton)) {
+      lock = 0;
+    }
+
+    if (globals::master.get_digital(cataRunButton)) {
+			runCata(-12000);
+		} else {
+			runCata(0);
+		}
+  }
+
+  void wingsControlSingle(pros::controller_digital_e_t wingControlButton) {
+    if (globals::master.get_digital_new_press(wingControlButton)) {
+      if (wings.getState() == 0) // A value of 0 indicates that both wings are closed
+        wings.open();
+      else if (wings.getState() == 3) // A value of 3 indicates that both wings are open
+        wings.close();
+    }
+  }
+
+  void toggleIntake(bool val) {
+    int valTo = (val == true) ? 1 : 0;
+    intake_piston.set_value(valTo);
+  }
+
+  // void toggleRemovalMech(bool val) {
+  //   int valTo = (val == true) ? 1 : 0;
+  //   doinker_piston.set_value(valTo);
+  // }
+
+  /*
+    Handle respective controls
+  */
+
+  void renu_control() {
+    cataControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
+    wingsControlSingle(RENU_WING_CONTROL);
+    intakeControl(RENU_INTAKE_CONTROL);
+  }
+  
+  void ria_control() {
+    cataControl(RIA_PTO_TOGGLE, RIA_CATA_CONTROL);
+    wingsControlSingle(RIA_WINGS_CONTROL);
+    intakeControl(RIA_INTAKE_CONTROL);
+  }
+
+  void chris_control() {
+    cataControl(RENU_PTO_TOGGLE, RENU_CATA_CONTROL);
+    wingsControlSingle(RENU_WING_CONTROL);
+    intakeControl(RENU_INTAKE_CONTROL);
+  }
+}
\ No newline at end of file
diff --git a/HVN-ON-EARTH/static/clear.gif b/HVN-ON-EARTH/static/clear.gif
new file mode 100644
index 0000000..1acb179
Binary files /dev/null and b/HVN-ON-EARTH/static/clear.gif differ