Initial commit - test serial

SerialTest/include/okapi/api/chassis/controller/chassisController.hpp

@@ -0,0 +1,142 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisScales.hpp"
+#include "okapi/api/chassis/model/chassisModel.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include <memory>
+#include <valarray>
+
+namespace okapi {
+class ChassisController {
+  public:
+  /**
+   * A ChassisController adds a closed-loop layer on top of a ChassisModel. moveDistance and
+   * turnAngle both use closed-loop control to move the robot. There are passthrough functions for
+   * everything defined in ChassisModel.
+   *
+   * @param imodel underlying ChassisModel
+   */
+  explicit ChassisController() = default;
+
+  virtual ~ChassisController() = default;
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * @param itarget distance to travel
+   */
+  virtual void moveDistance(QLength itarget) = 0;
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  virtual void moveRaw(double itarget) = 0;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel
+   */
+  virtual void moveDistanceAsync(QLength itarget) = 0;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  virtual void moveRawAsync(double itarget) = 0;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for
+   */
+  virtual void turnAngle(QAngle idegTarget) = 0;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  virtual void turnRaw(double idegTarget) = 0;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for
+   */
+  virtual void turnAngleAsync(QAngle idegTarget) = 0;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  virtual void turnRawAsync(double idegTarget) = 0;
+
+  /**
+   * Sets whether turns should be mirrored.
+   *
+   * @param ishouldMirror whether turns should be mirrored
+   */
+  virtual void setTurnsMirrored(bool ishouldMirror) = 0;
+
+  /**
+   * Checks whether the internal controllers are currently settled.
+   *
+   * @return Whether this ChassisController is settled.
+   */
+  virtual bool isSettled() = 0;
+
+  /**
+   * Delays until the currently executing movement completes.
+   */
+  virtual void waitUntilSettled() = 0;
+
+  /**
+   * Interrupts the current movement to stop the robot.
+   */
+  virtual void stop() = 0;
+
+  /**
+   * Sets a new maximum velocity in RPM [0-600].
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  virtual void setMaxVelocity(double imaxVelocity) = 0;
+
+  /**
+   * @return The maximum velocity in RPM [0-600].
+   */
+  virtual double getMaxVelocity() const = 0;
+
+  /**
+   * Get the ChassisScales.
+   */
+  virtual ChassisScales getChassisScales() const = 0;
+
+  /**
+   * Get the GearsetRatioPair.
+   */
+  virtual AbstractMotor::GearsetRatioPair getGearsetRatioPair() const = 0;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  virtual std::shared_ptr<ChassisModel> getModel() = 0;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  virtual ChassisModel &model() = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/controller/chassisControllerIntegrated.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 "okapi/api/chassis/controller/chassisController.hpp"
+#include "okapi/api/control/async/asyncPosIntegratedController.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+
+namespace okapi {
+class ChassisControllerIntegrated : public ChassisController {
+  public:
+  /**
+   * ChassisController using the V5 motor's integrated control. Puts the motors into encoder count
+   * units. Throws a `std::invalid_argument` exception if the gear ratio is zero. The initial
+   * model's max velocity will be propagated to the controllers.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The ChassisModel used to read from sensors/write to motors.
+   * @param ileftController The controller used for the left side motors.
+   * @param irightController The controller used for the right side motors.
+   * @param igearset The internal gearset and external ratio used on the drive motors.
+   * @param iscales The ChassisScales.
+   * @param ilogger The logger this instance will log to.
+   */
+  ChassisControllerIntegrated(
+    const TimeUtil &itimeUtil,
+    std::shared_ptr<ChassisModel> imodel,
+    std::unique_ptr<AsyncPosIntegratedController> ileftController,
+    std::unique_ptr<AsyncPosIntegratedController> irightController,
+    const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
+    const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * ```cpp
+   * // Drive forward 6 inches
+   * chassis->moveDistance(6_in);
+   *
+   * // Drive backward 0.2 meters
+   * chassis->moveDistance(-0.2_m);
+   * ```
+   *
+   * @param itarget distance to travel
+   */
+  void moveDistance(QLength itarget) override;
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * ```cpp
+   * // Drive forward by spinning the motors 400 degrees
+   * chassis->moveRaw(400);
+   * ```
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  void moveRaw(double itarget) override;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel
+   */
+  void moveDistanceAsync(QLength itarget) override;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  void moveRawAsync(double itarget) override;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * ```cpp
+   * // Turn 90 degrees clockwise
+   * chassis->turnAngle(90_deg);
+   * ```
+   *
+   * @param idegTarget angle to turn for
+   */
+  void turnAngle(QAngle idegTarget) override;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * ```cpp
+   * // Turn clockwise by spinning the motors 200 degrees
+   * chassis->turnRaw(200);
+   * ```
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  void turnRaw(double idegTarget) override;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for
+   */
+  void turnAngleAsync(QAngle idegTarget) override;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  void turnRawAsync(double idegTarget) override;
+
+  /**
+   * Sets whether turns should be mirrored.
+   *
+   * @param ishouldMirror whether turns should be mirrored
+   */
+  void setTurnsMirrored(bool ishouldMirror) override;
+
+  /**
+   * Checks whether the internal controllers are currently settled.
+   *
+   * @return Whether this ChassisController is settled.
+   */
+  bool isSettled() override;
+
+  /**
+   * Delays until the currently executing movement completes.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Interrupts the current movement to stop the robot.
+   */
+  void stop() override;
+
+  /**
+   * Get the ChassisScales.
+   */
+  ChassisScales getChassisScales() const override;
+
+  /**
+   * Get the GearsetRatioPair.
+   */
+  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  std::shared_ptr<ChassisModel> getModel() override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  ChassisModel &model() override;
+
+  /**
+   * Sets a new maximum velocity in RPM [0-600].
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The maximum velocity in RPM [0-600].
+   */
+  double getMaxVelocity() const override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  bool normalTurns{true};
+  std::shared_ptr<ChassisModel> chassisModel;
+  TimeUtil timeUtil;
+  std::unique_ptr<AsyncPosIntegratedController> leftController;
+  std::unique_ptr<AsyncPosIntegratedController> rightController;
+  int lastTarget;
+  ChassisScales scales;
+  AbstractMotor::GearsetRatioPair gearsetRatioPair;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/controller/chassisControllerPid.hpp

@@ -0,0 +1,275 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisController.hpp"
+#include "okapi/api/control/iterative/iterativePosPidController.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <atomic>
+#include <memory>
+#include <tuple>
+
+namespace okapi {
+class ChassisControllerPID : public ChassisController {
+  public:
+  /**
+   * ChassisController using PID control. Puts the motors into encoder count units. Throws a
+   * `std::invalid_argument` exception if the gear ratio is zero.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The ChassisModel used to read from sensors/write to motors.
+   * @param idistanceController The PID controller that controls chassis distance for driving
+   * straight.
+   * @param iturnController The PID controller that controls chassis angle for turning.
+   * @param iangleController The PID controller that controls chassis angle for driving straight.
+   * @param igearset The internal gearset and external ratio used on the drive motors.
+   * @param iscales The ChassisScales.
+   * @param ilogger The logger this instance will log to.
+   */
+  ChassisControllerPID(
+    TimeUtil itimeUtil,
+    std::shared_ptr<ChassisModel> imodel,
+    std::unique_ptr<IterativePosPIDController> idistanceController,
+    std::unique_ptr<IterativePosPIDController> iturnController,
+    std::unique_ptr<IterativePosPIDController> iangleController,
+    const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
+    const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  ChassisControllerPID(const ChassisControllerPID &) = delete;
+  ChassisControllerPID(ChassisControllerPID &&other) = delete;
+  ChassisControllerPID &operator=(const ChassisControllerPID &other) = delete;
+  ChassisControllerPID &operator=(ChassisControllerPID &&other) = delete;
+
+  ~ChassisControllerPID() override;
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * ```cpp
+   * // Drive forward 6 inches
+   * chassis->moveDistance(6_in);
+   *
+   * // Drive backward 0.2 meters
+   * chassis->moveDistance(-0.2_m);
+   * ```
+   *
+   * @param itarget distance to travel
+   */
+  void moveDistance(QLength itarget) override;
+
+  /**
+   * Drives the robot straight for a distance (using closed-loop control).
+   *
+   * ```cpp
+   * // Drive forward by spinning the motors 400 degrees
+   * chassis->moveRaw(400);
+   * ```
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  void moveRaw(double itarget) override;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel
+   */
+  void moveDistanceAsync(QLength itarget) override;
+
+  /**
+   * Sets the target distance for the robot to drive straight (using closed-loop control).
+   *
+   * @param itarget distance to travel in motor degrees
+   */
+  void moveRawAsync(double itarget) override;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * ```cpp
+   * // Turn 90 degrees clockwise
+   * chassis->turnAngle(90_deg);
+   * ```
+   *
+   * @param idegTarget angle to turn for
+   */
+  void turnAngle(QAngle idegTarget) override;
+
+  /**
+   * Turns the robot clockwise in place (using closed-loop control).
+   *
+   * ```cpp
+   * // Turn clockwise by spinning the motors 200 degrees
+   * chassis->turnRaw(200);
+   * ```
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  void turnRaw(double idegTarget) override;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for
+   */
+  void turnAngleAsync(QAngle idegTarget) override;
+
+  /**
+   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
+   *
+   * @param idegTarget angle to turn for in motor degrees
+   */
+  void turnRawAsync(double idegTarget) override;
+
+  /**
+   * Sets whether turns should be mirrored.
+   *
+   * @param ishouldMirror whether turns should be mirrored
+   */
+  void setTurnsMirrored(bool ishouldMirror) override;
+
+  /**
+   * Checks whether the internal controllers are currently settled.
+   *
+   * @return Whether this ChassisController is settled.
+   */
+  bool isSettled() override;
+
+  /**
+   * Delays until the currently executing movement completes.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Gets the ChassisScales.
+   */
+  ChassisScales getChassisScales() const override;
+
+  /**
+   * Gets the GearsetRatioPair.
+   */
+  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
+
+  /**
+   * Sets the velocity mode flag. When the controller is in velocity mode, the control loop will
+   * set motor velocities. When the controller is in voltage mode (`ivelocityMode = false`), the
+   * control loop will set motor voltages. Additionally, when the controller is in voltage mode,
+   * it will not obey maximum velocity limits.
+   *
+   * @param ivelocityMode Whether the controller should be in velocity or voltage mode.
+   */
+  void setVelocityMode(bool ivelocityMode);
+
+  /**
+   * Sets the gains for all controllers.
+   *
+   * @param idistanceGains The distance controller gains.
+   * @param iturnGains The turn controller gains.
+   * @param iangleGains The angle controller gains.
+   */
+  void setGains(const IterativePosPIDController::Gains &idistanceGains,
+                const IterativePosPIDController::Gains &iturnGains,
+                const IterativePosPIDController::Gains &iangleGains);
+
+  /**
+   * Gets the current controller gains.
+   *
+   * @return The current controller gains in the order: distance, turn, angle.
+   */
+  std::tuple<IterativePosPIDController::Gains,
+             IterativePosPIDController::Gains,
+             IterativePosPIDController::Gains>
+  getGains() const;
+
+  /**
+   * Starts the internal thread. This method is called by the ChassisControllerBuilder when making a
+   * new instance of this class.
+   */
+  void startThread();
+
+  /**
+   * Returns the underlying thread handle.
+   *
+   * @return The underlying thread handle.
+   */
+  CrossplatformThread *getThread() const;
+
+  /**
+   * Interrupts the current movement to stop the robot.
+   */
+  void stop() override;
+
+  /**
+   * Sets a new maximum velocity in RPM [0-600]. In voltage mode, the max velocity is ignored and a
+   * max voltage should be set on the underlying ChassisModel instead.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The maximum velocity in RPM [0-600].
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  std::shared_ptr<ChassisModel> getModel() override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  ChassisModel &model() override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  bool normalTurns{true};
+  std::shared_ptr<ChassisModel> chassisModel;
+  TimeUtil timeUtil;
+  std::unique_ptr<IterativePosPIDController> distancePid;
+  std::unique_ptr<IterativePosPIDController> turnPid;
+  std::unique_ptr<IterativePosPIDController> anglePid;
+  ChassisScales scales;
+  AbstractMotor::GearsetRatioPair gearsetRatioPair;
+  bool velocityMode{true};
+  std::atomic_bool doneLooping{true};
+  std::atomic_bool doneLoopingSeen{true};
+  std::atomic_bool newMovement{false};
+  std::atomic_bool dtorCalled{false};
+  QTime threadSleepTime{10_ms};
+
+  static void trampoline(void *context);
+  void loop();
+
+  /**
+   * Wait for the distance setup (distancePid and anglePid) to settle.
+   *
+   * @return true if done settling; false if settling should be tried again
+   */
+  bool waitForDistanceSettled();
+
+  /**
+   * Wait for the angle setup (anglePid) to settle.
+   *
+   * @return true if done settling; false if settling should be tried again
+   */
+  bool waitForAngleSettled();
+
+  /**
+   * Stops all the controllers and the ChassisModel.
+   */
+  void stopAfterSettled();
+
+  typedef enum { distance, angle, none } modeType;
+  modeType mode{none};
+
+  CrossplatformThread *task{nullptr};
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/controller/chassisScales.hpp

@@ -0,0 +1,88 @@
+/*
+ * 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 "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+#include "okapi/api/util/logging.hpp"
+#include <initializer_list>
+#include <stdexcept>
+#include <vector>
+
+namespace okapi {
+class ChassisScales {
+  public:
+  /**
+   * The scales a ChassisController needs to do all of its closed-loop control. The first element is
+   * the wheel diameter, the second element is the wheel track. For three-encoder configurations,
+   * the length from the center of rotation to the middle wheel and the middle wheel diameter are
+   * passed as the third and fourth elements.
+   *
+   * The wheel track is the center-to-center distance between the wheels (center-to-center
+   * meaning the width between the centers of both wheels). For example, if you are using four inch
+   * omni wheels and there are 11.5 inches between the centers of each wheel, you would call the
+   * constructor like so:
+   *   `ChassisScales scales({4_in, 11.5_in}, imev5GreenTPR); // imev5GreenTPR for a green gearset`
+   *
+   *                             Wheel diameter
+   *
+   *                              +-+      Center of rotation
+   *                              | |      |
+   *                              v v      +----------+ Length to middle wheel
+   *                                       |          | from center of rotation
+   *                     +--->    ===      |      === |
+   *                     |         +       v       +  |
+   *                     |        ++---------------++ |
+   *                     |        |                 | v
+   *       Wheel track   |        |                 |
+   *                     |        |        x        |+|  <-- Middle wheel
+   *                     |        |                 |
+   *                     |        |                 |
+   *                     |        ++---------------++
+   *                     |         +               +
+   *                     +--->    ===             ===
+   *
+   *
+   * @param idimensions {wheel diameter, wheel track} or {wheel diameter, wheel track, length to
+   * middle wheel, middle wheel diameter}.
+   * @param itpr The ticks per revolution of the encoders.
+   * @param ilogger The logger this instance will log to.
+   */
+  ChassisScales(const std::initializer_list<QLength> &idimensions,
+                double itpr,
+                const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * The scales a ChassisController needs to do all of its closed-loop control. The first element is
+   * the straight scale, the second element is the turn scale. Optionally, the length from the
+   * center of rotation to the middle wheel and the middle scale can be passed as the third and
+   * fourth elements. The straight scale converts motor degrees to meters, the turn scale converts
+   * motor degrees to robot turn degrees, and the middle scale converts middle wheel degrees to
+   * meters.
+   *
+   * @param iscales {straight scale, turn scale} or {straight scale, turn scale, length to middle
+   * wheel in meters, middle scale}.
+   * @param itpr The ticks per revolution of the encoders.
+   * @param ilogger The logger this instance will log to.
+   */
+  ChassisScales(const std::initializer_list<double> &iscales,
+                double itpr,
+                const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  QLength wheelDiameter;
+  QLength wheelTrack;
+  QLength middleWheelDistance;
+  QLength middleWheelDiameter;
+  double straight;
+  double turn;
+  double middle;
+  double tpr;
+
+  protected:
+  static void validateInputSize(std::size_t inputSize, const std::shared_ptr<Logger> &logger);
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/controller/defaultOdomChassisController.hpp

@@ -0,0 +1,183 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisControllerIntegrated.hpp"
+#include "okapi/api/chassis/controller/odomChassisController.hpp"
+#include "okapi/api/chassis/model/skidSteerModel.hpp"
+#include "okapi/api/odometry/odometry.hpp"
+#include <memory>
+
+namespace okapi {
+class DefaultOdomChassisController : public OdomChassisController {
+  public:
+  /**
+   * Odometry based chassis controller that moves using a separately constructed chassis controller.
+   * Spins up a task at the default priority plus 1 for odometry when constructed.
+   *
+   * Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
+   * turn some amount, you instead tell it to drive to a specific point on the field or turn to
+   * a specific angle, relative to its starting position.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param iodometry The odometry to read state estimates from.
+   * @param icontroller The chassis controller to delegate to.
+   * @param imode The new default StateMode used to interpret target points and query the Odometry
+   * state.
+   * @param imoveThreshold minimum length movement (smaller movements will be skipped)
+   * @param iturnThreshold minimum angle turn (smaller turns will be skipped)
+   * @param ilogger The logger this instance will log to.
+   */
+  DefaultOdomChassisController(const TimeUtil &itimeUtil,
+                               std::shared_ptr<Odometry> iodometry,
+                               std::shared_ptr<ChassisController> icontroller,
+                               const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
+                               QLength imoveThreshold = 0_mm,
+                               QAngle iturnThreshold = 0_deg,
+                               std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  DefaultOdomChassisController(const DefaultOdomChassisController &) = delete;
+  DefaultOdomChassisController(DefaultOdomChassisController &&other) = delete;
+  DefaultOdomChassisController &operator=(const DefaultOdomChassisController &other) = delete;
+  DefaultOdomChassisController &operator=(DefaultOdomChassisController &&other) = delete;
+
+  /**
+   * Drives the robot straight to a point in the odom frame.
+   *
+   * @param ipoint The target point to navigate to.
+   * @param ibackwards Whether to drive to the target point backwards.
+   * @param ioffset An offset from the target point in the direction pointing towards the robot. The
+   * robot will stop this far away from the target point.
+   */
+  void driveToPoint(const Point &ipoint,
+                    bool ibackwards = false,
+                    const QLength &ioffset = 0_mm) override;
+
+  /**
+   * Turns the robot to face a point in the odom frame.
+   *
+   * @param ipoint The target point to turn to face.
+   */
+  void turnToPoint(const Point &ipoint) override;
+
+  /**
+   * @return The internal ChassisController.
+   */
+  std::shared_ptr<ChassisController> getChassisController();
+
+  /**
+   * @return The internal ChassisController.
+   */
+  ChassisController &chassisController();
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnToAngle(const QAngle &iangle) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveDistance(QLength itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveRaw(double itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveDistanceAsync(QLength itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveRawAsync(double itarget) override;
+
+  /**
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle
+   */
+  void turnAngle(QAngle idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnRaw(double idegTarget) override;
+
+  /**
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle 
+   */
+  void turnAngleAsync(QAngle idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnRawAsync(double idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void setTurnsMirrored(bool ishouldMirror) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  bool isSettled() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void stop() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  ChassisScales getChassisScales() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  std::shared_ptr<ChassisModel> getModel() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  ChassisModel &model() override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::shared_ptr<ChassisController> controller;
+
+  void waitForOdomTask();
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/controller/odomChassisController.hpp

@@ -0,0 +1,154 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisController.hpp"
+#include "okapi/api/chassis/model/skidSteerModel.hpp"
+#include "okapi/api/coreProsAPI.hpp"
+#include "okapi/api/odometry/odometry.hpp"
+#include "okapi/api/odometry/point.hpp"
+#include "okapi/api/units/QSpeed.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <atomic>
+#include <memory>
+#include <valarray>
+
+namespace okapi {
+class OdomChassisController : public ChassisController {
+  public:
+  /**
+   * Odometry based chassis controller. Starts task at the default for odometry when constructed,
+   * which calls `Odometry::step` every `10ms`. The default StateMode is
+   * `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
+   * turn some amount, you instead tell it to drive to a specific point on the field or turn to
+   * a specific angle relative to its starting position.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param iodometry The Odometry instance to run in a new task.
+   * @param imode The new default StateMode used to interpret target points and query the Odometry
+   * state.
+   * @param imoveThreshold minimum length movement (smaller movements will be skipped)
+   * @param iturnThreshold minimum angle turn (smaller turns will be skipped)
+   */
+  OdomChassisController(TimeUtil itimeUtil,
+                        std::shared_ptr<Odometry> iodometry,
+                        const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
+                        const QLength &imoveThreshold = 0_mm,
+                        const QAngle &iturnThreshold = 0_deg,
+                        std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  ~OdomChassisController() override;
+
+  OdomChassisController(const OdomChassisController &) = delete;
+  OdomChassisController(OdomChassisController &&other) = delete;
+  OdomChassisController &operator=(const OdomChassisController &other) = delete;
+  OdomChassisController &operator=(OdomChassisController &&other) = delete;
+
+  /**
+   * Drives the robot straight to a point in the odom frame.
+   *
+   * @param ipoint The target point to navigate to.
+   * @param ibackwards Whether to drive to the target point backwards.
+   * @param ioffset An offset from the target point in the direction pointing towards the robot. The
+   * robot will stop this far away from the target point.
+   */
+  virtual void
+  driveToPoint(const Point &ipoint, bool ibackwards = false, const QLength &ioffset = 0_mm) = 0;
+
+  /**
+   * Turns the robot to face a point in the odom frame.
+   *
+   * @param ipoint The target point to turn to face.
+   */
+  virtual void turnToPoint(const Point &ipoint) = 0;
+
+  /**
+   * Turns the robot to face an angle in the odom frame.
+   *
+   * @param iangle The angle to turn to.
+   */
+  virtual void turnToAngle(const QAngle &iangle) = 0;
+
+  /**
+   * @return The current state.
+   */
+  virtual OdomState getState() const;
+
+  /**
+   * Set a new state to be the current state. The default StateMode is
+   * `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param istate The new state in the given format.
+   * @param imode The mode to treat the input state as.
+   */
+  virtual void setState(const OdomState &istate);
+
+  /**
+   * Sets a default StateMode that will be used to interpret target points and query the Odometry
+   * state.
+   *
+   * @param imode The new default StateMode.
+   */
+  void setDefaultStateMode(const StateMode &imode);
+
+  /**
+   * Set a new move threshold. Any requested movements smaller than this threshold will be skipped.
+   *
+   * @param imoveThreshold new move threshold
+   */
+  virtual void setMoveThreshold(const QLength &imoveThreshold);
+
+  /**
+   * Set a new turn threshold. Any requested turns smaller than this threshold will be skipped.
+   *
+   * @param iturnTreshold new turn threshold
+   */
+  virtual void setTurnThreshold(const QAngle &iturnTreshold);
+
+  /**
+   * @return The current move threshold.
+   */
+  virtual QLength getMoveThreshold() const;
+
+  /**
+   * @return The current turn threshold.
+   */
+  virtual QAngle getTurnThreshold() const;
+
+  /**
+   * Starts the internal odometry thread. This should not be called by normal users.
+   */
+  void startOdomThread();
+
+  /**
+   * @return The underlying thread handle.
+   */
+  CrossplatformThread *getOdomThread() const;
+
+  /**
+   * @return The internal odometry.
+   */
+  std::shared_ptr<Odometry> getOdometry();
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  TimeUtil timeUtil;
+  QLength moveThreshold;
+  QAngle turnThreshold;
+  std::shared_ptr<Odometry> odom;
+  CrossplatformThread *odomTask{nullptr};
+  std::atomic_bool dtorCalled{false};
+  StateMode defaultStateMode{StateMode::FRAME_TRANSFORMATION};
+  std::atomic_bool odomTaskRunning{false};
+
+  static void trampoline(void *context);
+  void loop();
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/chassisModel.hpp

@@ -0,0 +1,165 @@
+/*
+ * 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 "okapi/api/chassis/model/readOnlyChassisModel.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include <array>
+#include <initializer_list>
+#include <memory>
+#include <vector>
+
+namespace okapi {
+/**
+ * A version of the ReadOnlyChassisModel that also supports write methods, such as setting motor
+ * speed. Because this class can write to motors, there can only be one owner and as such copying
+ * is disabled.
+ */
+class ChassisModel : public ReadOnlyChassisModel {
+  public:
+  explicit ChassisModel() = default;
+  ChassisModel(const ChassisModel &) = delete;
+  ChassisModel &operator=(const ChassisModel &) = delete;
+
+  /**
+   * Drive the robot forwards (using open-loop control). Uses velocity mode.
+   *
+   * @param ipower motor power
+   */
+  virtual void forward(double ispeed) = 0;
+
+  /**
+   * Drive the robot in an arc (using open-loop control). Uses velocity mode.
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwadSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  virtual void driveVector(double iforwardSpeed, double iyaw) = 0;
+
+  /**
+   * Drive the robot in an arc. Uses voltage mode.
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwadSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  virtual void driveVectorVoltage(double iforwardSpeed, double iyaw) = 0;
+
+  /**
+   * Turn the robot clockwise (using open-loop control). Uses velocity mode.
+   *
+   * @param ispeed motor power
+   */
+  virtual void rotate(double ispeed) = 0;
+
+  /**
+   * Stop the robot (set all the motors to 0). Uses velocity mode.
+   */
+  virtual void stop() = 0;
+
+  /**
+   * Drive the robot with a tank drive layout. Uses voltage mode.
+   *
+   * @param ileftSpeed left side speed
+   * @param irightSpeed right side speed
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) = 0;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) = 0;
+
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) = 0;
+
+  /**
+   * Power the left side motors. Uses velocity mode.
+   *
+   * @param ipower motor power
+   */
+  virtual void left(double ispeed) = 0;
+
+  /**
+   * Power the right side motors. Uses velocity mode.
+   *
+   * @param ipower motor power
+   */
+  virtual void right(double ispeed) = 0;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  virtual void resetSensors() = 0;
+
+  /**
+   * Set the brake mode for each motor.
+   *
+   * @param mode new brake mode
+   */
+  virtual void setBrakeMode(AbstractMotor::brakeMode mode) = 0;
+
+  /**
+   * Set the encoder units for each motor.
+   *
+   * @param units new motor encoder units
+   */
+  virtual void setEncoderUnits(AbstractMotor::encoderUnits units) = 0;
+
+  /**
+   * Set the gearset for each motor.
+   *
+   * @param gearset new motor gearset
+   */
+  virtual void setGearing(AbstractMotor::gearset gearset) = 0;
+
+  /**
+   * Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
+   * currently installed gearset. If the configured maximum velocity is greater than the attainable
+   * maximum velocity from the currently installed gearset, the ChassisModel will still scale to
+   * that velocity.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  virtual void setMaxVelocity(double imaxVelocity) = 0;
+
+  /**
+   * @return The current maximum velocity.
+   */
+  virtual double getMaxVelocity() const = 0;
+
+  /**
+   * Sets a new maximum voltage in mV in the range `[0-12000]`.
+   *
+   * @param imaxVoltage The new maximum voltage.
+   */
+  virtual void setMaxVoltage(double imaxVoltage) = 0;
+
+  /**
+   * @return The maximum voltage in mV `[0-12000]`.
+   */
+  virtual double getMaxVoltage() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/hDriveModel.hpp

@@ -0,0 +1,247 @@
+/*
+ * 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 "okapi/api/chassis/model/chassisModel.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
+
+namespace okapi {
+class HDriveModel : public ChassisModel {
+  public:
+  /**
+   * Model for an h-drive (wheels parallel with robot's direction of motion, with an additional
+   * wheel perpendicular to those). When the left and right side motors are powered +100%, the robot
+   * should move forward in a straight line. When the middle motor is powered +100%, the robot
+   * should strafe right in a straight line.
+   *
+   * @param ileftSideMotor The left side motor.
+   * @param irightSideMotor The right side motor.
+   * @param imiddleMotor The middle (perpendicular) motor.
+   * @param ileftEnc The left side encoder.
+   * @param irightEnc The right side encoder.
+   * @param imiddleEnc The middle encoder.
+   */
+  HDriveModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
+              std::shared_ptr<AbstractMotor> irightSideMotor,
+              std::shared_ptr<AbstractMotor> imiddleMotor,
+              std::shared_ptr<ContinuousRotarySensor> ileftEnc,
+              std::shared_ptr<ContinuousRotarySensor> irightEnc,
+              std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
+              double imaxVelocity,
+              double imaxVoltage);
+
+  /**
+   * Drive the robot forwards (using open-loop control). Uses velocity mode. Sets the middle motor
+   * to zero velocity.
+   *
+   * @param ispeed motor power
+   */
+  void forward(double ispeed) override;
+
+  /**
+   * Drive the robot in an arc (using open-loop control). Uses velocity mode. Sets the middle motor
+   * to zero velocity.
+   *
+   * The algorithm is (approximately):
+   *    leftPower = ySpeed + zRotation
+   *    rightPower = ySpeed - zRotation
+   *
+   * @param iySpeed speed on y axis (forward)
+   * @param izRotation speed around z axis (up)
+   */
+  void driveVector(double iySpeed, double izRotation) override;
+
+  /**
+   * Drive the robot in an arc. Uses voltage mode. Sets the middle motor to zero velocity.
+   *
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwadSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
+
+  /**
+   * Turn the robot clockwise (using open-loop control). Uses velocity mode. Sets the middle motor
+   * to zero velocity.
+   *
+   * @param ispeed motor power
+   */
+  void rotate(double ispeed) override;
+
+  /**
+   * Stop the robot (set all the motors to 0). Uses velocity mode.
+   */
+  void stop() override;
+
+  /**
+   * Drive the robot with a tank drive layout. Uses voltage mode. Sets the middle motor to zero
+   * velocity.
+   *
+   * @param ileftSpeed left side speed
+   * @param irightSpeed right side speed
+   * @param ithreshold deadband on joystick values
+   */
+  void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode. Sets the middle motor to zero
+   * velocity.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode. Sets the middle motor to zero velocity.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode.
+   *
+   * @param irightSpeed speed to the right
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void
+  hArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
+
+  /**
+   * Drive the robot with an curvature drive layout. Uses voltage mode.
+   *
+   * @param irightSpeed speed to the right
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void
+  hCurvature(double irightSpeed, double iforwardSpeed, double icurvature, double ithreshold = 0);
+
+  /**
+   * Power the left side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void left(double ispeed) override;
+
+  /**
+   * Power the right side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void right(double ispeed) override;
+
+  /**
+   * Power the middle motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  virtual void middle(double ispeed);
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings in the format {left, right, middle}
+   */
+  std::valarray<std::int32_t> getSensorVals() const override;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  void resetSensors() override;
+
+  /**
+   * Set the brake mode for each motor.
+   *
+   * @param mode new brake mode
+   */
+  void setBrakeMode(AbstractMotor::brakeMode mode) override;
+
+  /**
+   * Set the encoder units for each motor.
+   *
+   * @param units new motor encoder units
+   */
+  void setEncoderUnits(AbstractMotor::encoderUnits units) override;
+
+  /**
+   * Set the gearset for each motor.
+   *
+   * @param gearset new motor gearset
+   */
+  void setGearing(AbstractMotor::gearset gearset) override;
+
+  /**
+   * Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
+   * currently installed gearset. If the configured maximum velocity is greater than the attainable
+   * maximum velocity from the currently installed gearset, the ChassisModel will still scale to
+   * that velocity.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The current maximum velocity.
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * Sets a new maximum voltage in mV in the range `[0-12000]`.
+   *
+   * @param imaxVoltage The new maximum voltage.
+   */
+  void setMaxVoltage(double imaxVoltage) override;
+
+  /**
+   * @return The maximum voltage in mV in the range `[0-12000]`.
+   */
+  double getMaxVoltage() const override;
+
+  /**
+   * Returns the left side motor.
+   *
+   * @return the left side motor
+   */
+  std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
+
+  /**
+   * Returns the left side motor.
+   *
+   * @return the left side motor
+   */
+  std::shared_ptr<AbstractMotor> getRightSideMotor() const;
+
+  /**
+   * @return The middle motor.
+   */
+  std::shared_ptr<AbstractMotor> getMiddleMotor() const;
+
+  protected:
+  double maxVelocity;
+  double maxVoltage;
+  std::shared_ptr<AbstractMotor> leftSideMotor;
+  std::shared_ptr<AbstractMotor> rightSideMotor;
+  std::shared_ptr<AbstractMotor> middleMotor;
+  std::shared_ptr<ContinuousRotarySensor> leftSensor;
+  std::shared_ptr<ContinuousRotarySensor> rightSensor;
+  std::shared_ptr<ContinuousRotarySensor> middleSensor;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/readOnlyChassisModel.hpp

@@ -0,0 +1,28 @@
+/*
+ * 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 "okapi/api/coreProsAPI.hpp"
+#include <valarray>
+
+namespace okapi {
+/**
+ * A version of the ChassisModel that only supports read methods, such as querying sensor values.
+ * This class does not let you write to motors, so it supports having multiple owners and as a
+ * result copying is enabled.
+ */
+class ReadOnlyChassisModel {
+  public:
+  virtual ~ReadOnlyChassisModel() = default;
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings (format is implementation dependent)
+   */
+  virtual std::valarray<std::int32_t> getSensorVals() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/skidSteerModel.hpp

@@ -0,0 +1,198 @@
+/*
+ * 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 "okapi/api/chassis/model/chassisModel.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
+
+namespace okapi {
+class SkidSteerModel : public ChassisModel {
+  public:
+  /**
+   * Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
+   * motors are powered +100%, the robot should move forward in a straight line.
+   *
+   * @param ileftSideMotor The left side motor.
+   * @param irightSideMotor The right side motor.
+   * @param ileftEnc The left side encoder.
+   * @param irightEnc The right side encoder.
+   */
+  SkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
+                 std::shared_ptr<AbstractMotor> irightSideMotor,
+                 std::shared_ptr<ContinuousRotarySensor> ileftEnc,
+                 std::shared_ptr<ContinuousRotarySensor> irightEnc,
+                 double imaxVelocity,
+                 double imaxVoltage);
+
+  /**
+   * Drive the robot forwards (using open-loop control). Uses velocity mode.
+   *
+   * @param ispeed motor power
+   */
+  void forward(double ispeed) override;
+
+  /**
+   * Drive the robot in an arc (using open-loop control). Uses velocity mode.
+   * The algorithm is (approximately):
+   *   leftPower = ySpeed + zRotation
+   *   rightPower = ySpeed - zRotation
+   *
+   * @param iySpeed speed on y axis (forward)
+   * @param izRotation speed around z axis (up)
+   */
+  void driveVector(double iySpeed, double izRotation) override;
+
+  /**
+   * Drive the robot in an arc. Uses voltage mode.
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwadSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
+
+  /**
+   * Turn the robot clockwise (using open-loop control). Uses velocity mode.
+   *
+   * @param ispeed motor power
+   */
+  void rotate(double ispeed) override;
+
+  /**
+   * Stop the robot (set all the motors to 0). Uses velocity mode.
+   */
+  void stop() override;
+
+  /**
+   * Drive the robot with a tank drive layout. Uses voltage mode.
+   *
+   * @param ileftSpeed left side speed
+   * @param irightSpeed right side speed
+   * @param ithreshold deadband on joystick values
+   */
+  void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
+  /**
+   * Power the left side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void left(double ispeed) override;
+
+  /**
+   * Power the right side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void right(double ispeed) override;
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings in the format {left, right}
+   */
+  std::valarray<std::int32_t> getSensorVals() const override;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  void resetSensors() override;
+
+  /**
+   * Set the brake mode for each motor.
+   *
+   * @param mode new brake mode
+   */
+  void setBrakeMode(AbstractMotor::brakeMode mode) override;
+
+  /**
+   * Set the encoder units for each motor.
+   *
+   * @param units new motor encoder units
+   */
+  void setEncoderUnits(AbstractMotor::encoderUnits units) override;
+
+  /**
+   * Set the gearset for each motor.
+   *
+   * @param gearset new motor gearset
+   */
+  void setGearing(AbstractMotor::gearset gearset) override;
+
+  /**
+   * Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
+   * currently installed gearset. If the configured maximum velocity is greater than the attainable
+   * maximum velocity from the currently installed gearset, the ChassisModel will still scale to
+   * that velocity.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The current maximum velocity.
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * Sets a new maximum voltage in mV in the range `[0-12000]`.
+   *
+   * @param imaxVoltage The new maximum voltage.
+   */
+  void setMaxVoltage(double imaxVoltage) override;
+
+  /**
+   * @return The maximum voltage in mV in the range `[0-12000]`.
+   */
+  double getMaxVoltage() const override;
+
+  /**
+   * Returns the left side motor.
+   *
+   * @return the left side motor
+   */
+  std::shared_ptr<AbstractMotor> getLeftSideMotor() const;
+
+  /**
+   * Returns the left side motor.
+   *
+   * @return the left side motor
+   */
+  std::shared_ptr<AbstractMotor> getRightSideMotor() const;
+
+  protected:
+  double maxVelocity;
+  double maxVoltage;
+  std::shared_ptr<AbstractMotor> leftSideMotor;
+  std::shared_ptr<AbstractMotor> rightSideMotor;
+  std::shared_ptr<ContinuousRotarySensor> leftSensor;
+  std::shared_ptr<ContinuousRotarySensor> rightSensor;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp

@@ -0,0 +1,46 @@
+/*
+ * 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 "okapi/api/chassis/model/skidSteerModel.hpp"
+
+namespace okapi {
+class ThreeEncoderSkidSteerModel : public SkidSteerModel {
+  public:
+  /**
+   * Model for a skid steer drive (wheels parallel with robot's direction of motion). When all
+   * motors are powered +127, the robot should move forward in a straight line.
+   *
+   * @param ileftSideMotor left side motor
+   * @param irightSideMotor right side motor
+   * @param ileftEnc left side encoder
+   * @param imiddleEnc middle encoder (mounted perpendicular to the left and right side encoders)
+   * @param irightEnc right side encoder
+   */
+  ThreeEncoderSkidSteerModel(std::shared_ptr<AbstractMotor> ileftSideMotor,
+                             std::shared_ptr<AbstractMotor> irightSideMotor,
+                             std::shared_ptr<ContinuousRotarySensor> ileftEnc,
+                             std::shared_ptr<ContinuousRotarySensor> irightEnc,
+                             std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
+                             double imaxVelocity,
+                             double imaxVoltage);
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings in the format {left, right, middle}
+   */
+  std::valarray<std::int32_t> getSensorVals() const override;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  void resetSensors() override;
+
+  protected:
+  std::shared_ptr<ContinuousRotarySensor> middleSensor;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/threeEncoderXDriveModel.hpp

@@ -0,0 +1,50 @@
+/*
+ * 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 "okapi/api/chassis/model/xDriveModel.hpp"
+
+namespace okapi {
+class ThreeEncoderXDriveModel : public XDriveModel {
+  public:
+  /**
+   * Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
+   * +100%, the robot should move forward in a straight line.
+   *
+   * @param itopLeftMotor The top left motor.
+   * @param itopRightMotor The top right motor.
+   * @param ibottomRightMotor The bottom right motor.
+   * @param ibottomLeftMotor The bottom left motor.
+   * @param ileftEnc The left side encoder.
+   * @param irightEnc The right side encoder.
+   * @param imiddleEnc The middle encoder.
+   */
+  ThreeEncoderXDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
+                          std::shared_ptr<AbstractMotor> itopRightMotor,
+                          std::shared_ptr<AbstractMotor> ibottomRightMotor,
+                          std::shared_ptr<AbstractMotor> ibottomLeftMotor,
+                          std::shared_ptr<ContinuousRotarySensor> ileftEnc,
+                          std::shared_ptr<ContinuousRotarySensor> irightEnc,
+                          std::shared_ptr<ContinuousRotarySensor> imiddleEnc,
+                          double imaxVelocity,
+                          double imaxVoltage);
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings in the format {left, right, middle}
+   */
+  std::valarray<std::int32_t> getSensorVals() const override;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  void resetSensors() override;
+
+  protected:
+  std::shared_ptr<ContinuousRotarySensor> middleSensor;
+};
+} // namespace okapi

SerialTest/include/okapi/api/chassis/model/xDriveModel.hpp

@@ -0,0 +1,273 @@
+/*
+ * 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 "okapi/api/chassis/model/chassisModel.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
+#include "okapi/api/units/QAngle.hpp"
+
+namespace okapi {
+class XDriveModel : public ChassisModel {
+  public:
+  /**
+   * Model for an x drive (wheels at 45 deg from a skid steer drive). When all motors are powered
+   * +100%, the robot should move forward in a straight line.
+   *
+   * @param itopLeftMotor The top left motor.
+   * @param itopRightMotor The top right motor.
+   * @param ibottomRightMotor The bottom right motor.
+   * @param ibottomLeftMotor The bottom left motor.
+   * @param ileftEnc The left side encoder.
+   * @param irightEnc The right side encoder.
+   */
+  XDriveModel(std::shared_ptr<AbstractMotor> itopLeftMotor,
+              std::shared_ptr<AbstractMotor> itopRightMotor,
+              std::shared_ptr<AbstractMotor> ibottomRightMotor,
+              std::shared_ptr<AbstractMotor> ibottomLeftMotor,
+              std::shared_ptr<ContinuousRotarySensor> ileftEnc,
+              std::shared_ptr<ContinuousRotarySensor> irightEnc,
+              double imaxVelocity,
+              double imaxVoltage);
+
+  /**
+   * Drive the robot forwards (using open-loop control). Uses velocity mode.
+   *
+   * @param ispeed motor power
+   */
+  void forward(double ipower) override;
+
+  /**
+   * Drive the robot in an arc (using open-loop control). Uses velocity mode.
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  void driveVector(double iforwardSpeed, double iyaw) override;
+
+  /**
+   * Drive the robot in an arc. Uses voltage mode.
+   * The algorithm is (approximately):
+   *   leftPower = forwardSpeed + yaw
+   *   rightPower = forwardSpeed - yaw
+   *
+   * @param iforwadSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   */
+  void driveVectorVoltage(double iforwardSpeed, double iyaw) override;
+
+  /**
+   * Turn the robot clockwise (using open-loop control). Uses velocity mode.
+   *
+   * @param ipower motor power
+   */
+  void rotate(double ipower) override;
+
+  /**
+   * Drive the robot side-ways (using open-loop control) where positive ipower is
+   * to the right and negative ipower is to the left. Uses velocity mode.
+   *
+   * @param ispeed motor power
+   */
+  void strafe(double ipower);
+
+  /**
+   * Strafe the robot in an arc (using open-loop control) where positive istrafeSpeed is
+   * to the right and negative istrafeSpeed is to the left. Uses velocity mode.
+   * The algorithm is (approximately):
+   *   topLeftPower = -1 * istrafeSpeed + yaw
+   *   topRightPower = istrafeSpeed - yaw
+   *   bottomRightPower = -1 * istrafeSpeed - yaw
+   *   bottomLeftPower = istrafeSpeed + yaw
+   *
+   * @param istrafeSpeed speed to the right
+   * @param iyaw speed around the vertical axis
+   */
+  void strafeVector(double istrafeSpeed, double iyaw);
+
+  /**
+   * Stop the robot (set all the motors to 0). Uses velocity mode.
+   */
+  void stop() override;
+
+  /**
+   * Drive the robot with a tank drive layout. Uses voltage mode.
+   *
+   * @param ileftSpeed left side speed
+   * @param irightSpeed right side speed
+   * @param ithreshold deadband on joystick values
+   */
+  void tank(double ileftSpeed, double irightSpeed, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  void arcade(double iforwardSpeed, double iyaw, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with a curvature drive layout. The robot drives in constant radius turns
+   * where you control the curvature (inverse of radius) you drive in. This is advantageous
+   * because the forward speed will not affect the rate of turning. The algorithm switches to
+   * arcade if the forward speed is 0. Uses voltage mode.
+   *
+   * @param iforwardSpeed speed forward direction
+   * @param icurvature curvature (inverse of radius) to drive in
+   * @param ithreshold deadband on joystick values
+   */
+  void curvature(double iforwardSpeed, double icurvature, double ithreshold = 0) override;
+
+  /**
+   * Drive the robot with an arcade drive layout. Uses voltage mode.
+   *
+   * @param irightSpeed speed to the right
+   * @param iforwardSpeed speed in the forward direction
+   * @param iyaw speed around the vertical axis
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void
+  xArcade(double irightSpeed, double iforwardSpeed, double iyaw, double ithreshold = 0);
+
+  /**
+   * Drive the robot with a field-oriented arcade drive layout. Uses voltage mode.
+   * For example:
+   *    Both `fieldOrientedXArcade(1, 0, 0, 0_deg)` and `fieldOrientedXArcade(1, 0, 0, 90_deg)`
+   *    will drive the chassis in the forward/north direction. In other words, no matter
+   *    the robot's heading, the robot will move forward/north when you tell it
+   *    to move forward/north and will move right/east when you tell it to move right/east.
+   *
+   *
+   * @param ixSpeed forward speed -- (`+1`) forward, (`-1`) backward
+   * @param iySpeed sideways speed -- (`+1`) right, (`-1`) left
+   * @param iyaw turn speed -- (`+1`) clockwise, (`-1`) counter-clockwise
+   * @param iangle current chassis angle (`0_deg` = no correction, winds clockwise)
+   * @param ithreshold deadband on joystick values
+   */
+  virtual void fieldOrientedXArcade(double ixSpeed,
+                                    double iySpeed,
+                                    double iyaw,
+                                    QAngle iangle,
+                                    double ithreshold = 0);
+
+  /**
+   * Power the left side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void left(double ispeed) override;
+
+  /**
+   * Power the right side motors. Uses velocity mode.
+   *
+   * @param ispeed The motor power.
+   */
+  void right(double ispeed) override;
+
+  /**
+   * Read the sensors.
+   *
+   * @return sensor readings in the format {left, right}
+   */
+  std::valarray<std::int32_t> getSensorVals() const override;
+
+  /**
+   * Reset the sensors to their zero point.
+   */
+  void resetSensors() override;
+
+  /**
+   * Set the brake mode for each motor.
+   *
+   * @param mode new brake mode
+   */
+  void setBrakeMode(AbstractMotor::brakeMode mode) override;
+
+  /**
+   * Set the encoder units for each motor.
+   *
+   * @param units new motor encoder units
+   */
+  void setEncoderUnits(AbstractMotor::encoderUnits units) override;
+
+  /**
+   * Set the gearset for each motor.
+   *
+   * @param gearset new motor gearset
+   */
+  void setGearing(AbstractMotor::gearset gearset) override;
+
+  /**
+   * Sets a new maximum velocity in RPM. The usable maximum depends on the maximum velocity of the
+   * currently installed gearset. If the configured maximum velocity is greater than the attainable
+   * maximum velocity from the currently installed gearset, the ChassisModel will still scale to
+   * that velocity.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The current maximum velocity.
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * Sets a new maximum voltage in mV in the range `[0-12000]`.
+   *
+   * @param imaxVoltage The new maximum voltage.
+   */
+  void setMaxVoltage(double imaxVoltage) override;
+
+  /**
+   * @return The maximum voltage in mV in the range `[0-12000]`.
+   */
+  double getMaxVoltage() const override;
+
+  /**
+   * Returns the top left motor.
+   *
+   * @return the top left motor
+   */
+  std::shared_ptr<AbstractMotor> getTopLeftMotor() const;
+
+  /**
+   * Returns the top right motor.
+   *
+   * @return the top right motor
+   */
+  std::shared_ptr<AbstractMotor> getTopRightMotor() const;
+
+  /**
+   * Returns the bottom right motor.
+   *
+   * @return the bottom right motor
+   */
+  std::shared_ptr<AbstractMotor> getBottomRightMotor() const;
+
+  /**
+   * Returns the bottom left motor.
+   *
+   * @return the bottom left motor
+   */
+  std::shared_ptr<AbstractMotor> getBottomLeftMotor() const;
+
+  protected:
+  double maxVelocity;
+  double maxVoltage;
+  std::shared_ptr<AbstractMotor> topLeftMotor;
+  std::shared_ptr<AbstractMotor> topRightMotor;
+  std::shared_ptr<AbstractMotor> bottomRightMotor;
+  std::shared_ptr<AbstractMotor> bottomLeftMotor;
+  std::shared_ptr<ContinuousRotarySensor> leftSensor;
+  std::shared_ptr<ContinuousRotarySensor> rightSensor;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncController.hpp

@@ -0,0 +1,24 @@
+/*
+ * 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 "okapi/api/control/closedLoopController.hpp"
+
+namespace okapi {
+/**
+ * Closed-loop controller that steps on its own in another thread and automatically writes to the
+ * output.
+ */
+template <typename Input, typename Output>
+class AsyncController : public ClosedLoopController<Input, Output> {
+  public:
+  /**
+   * Blocks the current task until the controller has settled. Determining what settling means is
+   * implementation-dependent.
+   */
+  virtual void waitUntilSettled() = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncLinearMotionProfileController.hpp

@@ -0,0 +1,291 @@
+/*
+ * 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 "okapi/api/control/async/asyncPositionController.hpp"
+#include "okapi/api/control/util/pathfinderUtil.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/units/QAngularSpeed.hpp"
+#include "okapi/api/units/QSpeed.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <atomic>
+#include <map>
+
+#include "squiggles.hpp"
+
+namespace okapi {
+class AsyncLinearMotionProfileController : public AsyncPositionController<std::string, double> {
+  public:
+  /**
+   * An Async Controller which generates and follows 1D motion profiles.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param ilimits The default limits.
+   * @param ioutput The output to write velocity targets to.
+   * @param idiameter The effective diameter for whatever the motor spins.
+   * @param ipair The gearset.
+   * @param ilogger The logger this instance will log to.
+   */
+  AsyncLinearMotionProfileController(
+    const TimeUtil &itimeUtil,
+    const PathfinderLimits &ilimits,
+    const std::shared_ptr<ControllerOutput<double>> &ioutput,
+    const QLength &idiameter,
+    const AbstractMotor::GearsetRatioPair &ipair,
+    const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  AsyncLinearMotionProfileController(AsyncLinearMotionProfileController &&other) = delete;
+
+  AsyncLinearMotionProfileController &
+  operator=(AsyncLinearMotionProfileController &&other) = delete;
+
+  ~AsyncLinearMotionProfileController() override;
+
+  /**
+   * Generates a path which intersects the given waypoints and saves it internally with a key of
+   * pathId. Call `executePath()` with the same `pathId` to run it.
+   *
+   * If the waypoints form a path which is impossible to achieve, an instance of
+   * `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
+   * are no waypoints, no path is generated.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ipathId A unique identifier to save the path with.
+   */
+  void generatePath(std::initializer_list<QLength> iwaypoints, const std::string &ipathId);
+
+  /**
+   * Generates a path which intersects the given waypoints and saves it internally with a key of
+   * pathId. Call `executePath()` with the same pathId to run it.
+   *
+   * If the waypoints form a path which is impossible to achieve, an instance of
+   * `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
+   * are no waypoints, no path is generated.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ipathId A unique identifier to save the path with.
+   * @param ilimits The limits to use for this path only.
+   */
+  void generatePath(std::initializer_list<QLength> iwaypoints,
+                    const std::string &ipathId,
+                    const PathfinderLimits &ilimits);
+
+  /**
+   * Removes a path and frees the memory it used. This function returns `true` if the path was
+   * either deleted or didn't exist in the first place. It returns `false` if the path could not be
+   * removed because it is running.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to generatePath()
+   * @return `true` if the path no longer exists
+   */
+  bool removePath(const std::string &ipathId);
+
+  /**
+   * Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
+   *
+   * @return The identifiers of all paths
+   */
+  std::vector<std::string> getPaths();
+
+  /**
+   * Executes a path with the given ID. If there is no path matching the ID, the method will
+   * return. Any targets set while a path is being followed will be ignored.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
+   */
+  void setTarget(std::string ipathId) override;
+
+  /**
+   * Executes a path with the given ID. If there is no path matching the ID, the method will
+   * return. Any targets set while a path is being followed will be ignored.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
+   * @param ibackwards Whether to follow the profile backwards.
+   */
+  void setTarget(std::string ipathId, bool ibackwards);
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller.
+   *
+   * This just calls `setTarget()`.
+   */
+  void controllerSet(std::string ivalue) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  std::string getTarget() override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  virtual std::string getTarget() const;
+
+  /**
+   * This is overridden to return the current path.
+   *
+   * @return The most recent value of the process variable.
+   */
+  std::string getProcessValue() const override;
+
+  /**
+   * Blocks the current task until the controller has settled. This controller is settled when
+   * it has finished following a path. If no path is being followed, it is settled.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Generates a new path from the position (typically the current position) to the target and
+   * blocks until the controller has settled. Does not save the path which was generated.
+   *
+   * @param iposition The starting position.
+   * @param itarget The target position.
+   * @param ibackwards Whether to follow the profile backwards.
+   */
+  void moveTo(const QLength &iposition, const QLength &itarget, bool ibackwards = false);
+
+  /**
+   * Generates a new path from the position (typically the current position) to the target and
+   * blocks until the controller has settled. Does not save the path which was generated.
+   *
+   * @param iposition The starting position.
+   * @param itarget The target position.
+   * @param ilimits The limits to use for this path only.
+   * @param ibackwards Whether to follow the profile backwards.
+   */
+  void moveTo(const QLength &iposition,
+              const QLength &itarget,
+              const PathfinderLimits &ilimits,
+              bool ibackwards = false);
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled. Returns zero if there
+   * is no path currently being followed.
+   *
+   * @return the last error
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return `true`.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information. This implementation also stops movement.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * NOT cause the controller to move to its last set target.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * NOT cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * This implementation does nothing because the API always requires the starting position to be
+   * specified.
+   */
+  void tarePosition() override;
+
+  /**
+   * This implementation does nothing because the maximum velocity is configured using
+   * PathfinderLimits elsewhere.
+   *
+   * @param imaxVelocity Ignored.
+   */
+  void setMaxVelocity(std::int32_t imaxVelocity) override;
+
+  /**
+   * Starts the internal thread. This should not be called by normal users. This method is called
+   * by the AsyncControllerFactory when making a new instance of this class.
+   */
+  void startThread();
+
+  /**
+   * Returns the underlying thread handle.
+   *
+   * @return The underlying thread handle.
+   */
+  CrossplatformThread *getThread() const;
+
+  /**
+   * Attempts to remove a path without stopping execution, then if that fails, disables the
+   * controller and removes the path.
+   *
+   * @param ipathId The path ID that will be removed
+   */
+  void forceRemovePath(const std::string &ipathId);
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
+  PathfinderLimits limits;
+  std::shared_ptr<ControllerOutput<double>> output;
+  QLength diameter;
+  AbstractMotor::GearsetRatioPair pair;
+  double currentProfilePosition{0};
+  TimeUtil timeUtil;
+
+  // This must be locked when accessing the current path
+  CrossplatformMutex currentPathMutex;
+
+  std::string currentPath{""};
+  std::atomic_bool isRunning{false};
+  std::atomic_int direction{1};
+  std::atomic_bool disabled{false};
+  std::atomic_bool dtorCalled{false};
+  CrossplatformThread *task{nullptr};
+
+  static void trampoline(void *context);
+  void loop();
+
+  /**
+   * Follow the supplied path. Must follow the disabled lifecycle.
+   */
+  virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
+                                 std::unique_ptr<AbstractRate> rate);
+
+  /**
+   * Converts linear "chassis" speed to rotational motor speed.
+   *
+   * @param linear "chassis" frame speed
+   * @return motor frame speed
+   */
+  QAngularSpeed convertLinearToRotational(QSpeed linear) const;
+
+  std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
+                                  const std::string &ipathId,
+                                  int length);
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncMotionProfileController.hpp

@@ -0,0 +1,326 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisScales.hpp"
+#include "okapi/api/chassis/model/skidSteerModel.hpp"
+#include "okapi/api/control/async/asyncPositionController.hpp"
+#include "okapi/api/control/util/pathfinderUtil.hpp"
+#include "okapi/api/units/QAngularSpeed.hpp"
+#include "okapi/api/units/QSpeed.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <atomic>
+#include <iostream>
+#include <map>
+
+#include "squiggles.hpp"
+
+namespace okapi {
+class AsyncMotionProfileController : public AsyncPositionController<std::string, PathfinderPoint> {
+  public:
+  /**
+   * An Async Controller which generates and follows 2D motion profiles. Throws a
+   * `std::invalid_argument` exception if the gear ratio is zero.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param ilimits The default limits.
+   * @param imodel The chassis model to control.
+   * @param iscales The chassis dimensions.
+   * @param ipair The gearset.
+   * @param ilogger The logger this instance will log to.
+   */
+  AsyncMotionProfileController(const TimeUtil &itimeUtil,
+                               const PathfinderLimits &ilimits,
+                               const std::shared_ptr<ChassisModel> &imodel,
+                               const ChassisScales &iscales,
+                               const AbstractMotor::GearsetRatioPair &ipair,
+                               const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  AsyncMotionProfileController(AsyncMotionProfileController &&other) = delete;
+
+  AsyncMotionProfileController &operator=(AsyncMotionProfileController &&other) = delete;
+
+  ~AsyncMotionProfileController() override;
+
+  /**
+   * Generates a path which intersects the given waypoints and saves it internally with a key of
+   * pathId. Call `executePath()` with the same pathId to run it.
+   *
+   * If the waypoints form a path which is impossible to achieve, an instance of
+   * `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
+   * are no waypoints, no path is generated.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ipathId A unique identifier to save the path with.
+   */
+  void generatePath(std::initializer_list<PathfinderPoint> iwaypoints, const std::string &ipathId);
+
+  /**
+   * Generates a path which intersects the given waypoints and saves it internally with a key of
+   * pathId. Call `executePath()` with the same pathId to run it.
+   *
+   * If the waypoints form a path which is impossible to achieve, an instance of
+   * `std::runtime_error` is thrown (and an error is logged) which describes the waypoints. If there
+   * are no waypoints, no path is generated.
+   *
+   * NOTE: The waypoints are expected to be in the
+   * okapi::State::FRAME_TRANSFORMATION format where +x is forward, +y is right,
+   * and 0 theta is measured from the +x axis to the +y axis.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ipathId A unique identifier to save the path with.
+   * @param ilimits The limits to use for this path only.
+   */
+  void generatePath(std::initializer_list<PathfinderPoint> iwaypoints,
+                    const std::string &ipathId,
+                    const PathfinderLimits &ilimits);
+
+  /**
+   * Removes a path and frees the memory it used. This function returns true if the path was either
+   * deleted or didn't exist in the first place. It returns false if the path could not be removed
+   * because it is running.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to `generatePath()`
+   * @return True if the path no longer exists
+   */
+  bool removePath(const std::string &ipathId);
+
+  /**
+   * Gets the identifiers of all paths saved in this `AsyncMotionProfileController`.
+   *
+   * @return The identifiers of all paths
+   */
+  std::vector<std::string> getPaths();
+
+  /**
+   * Executes a path with the given ID. If there is no path matching the ID, the method will
+   * return. Any targets set while a path is being followed will be ignored.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
+   */
+  void setTarget(std::string ipathId) override;
+
+  /**
+   * Executes a path with the given ID. If there is no path matching the ID, the method will
+   * return. Any targets set while a path is being followed will be ignored.
+   *
+   * @param ipathId A unique identifier for the path, previously passed to `generatePath()`.
+   * @param ibackwards Whether to follow the profile backwards.
+   * @param imirrored Whether to follow the profile mirrored.
+   */
+  void setTarget(std::string ipathId, bool ibackwards, bool imirrored = false);
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. This just calls `setTarget()`.
+   */
+  void controllerSet(std::string ivalue) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  std::string getTarget() override;
+
+  /**
+   * This is overridden to return the current path.
+   *
+   * @return The most recent value of the process variable.
+   */
+  std::string getProcessValue() const override;
+
+  /**
+   * Blocks the current task until the controller has settled. This controller is settled when
+   * it has finished following a path. If no path is being followed, it is settled.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Generates a new path from the position (typically the current position) to the target and
+   * blocks until the controller has settled. Does not save the path which was generated.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ibackwards Whether to follow the profile backwards.
+   * @param imirrored Whether to follow the profile mirrored.
+   */
+  void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
+              bool ibackwards = false,
+              bool imirrored = false);
+
+  /**
+   * Generates a new path from the position (typically the current position) to the target and
+   * blocks until the controller has settled. Does not save the path which was generated.
+   *
+   * @param iwaypoints The waypoints to hit on the path.
+   * @param ilimits The limits to use for this path only.
+   * @param ibackwards Whether to follow the profile backwards.
+   * @param imirrored Whether to follow the profile mirrored.
+   */
+  void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
+              const PathfinderLimits &ilimits,
+              bool ibackwards = false,
+              bool imirrored = false);
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled. This implementation
+   * always returns zero since the robot is assumed to perfectly follow the path. Subclasses can
+   * override this to be more accurate using odometry information.
+   *
+   * @return the last error
+   */
+  PathfinderPoint getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Resets the controller so it can start from 0 again properly. Keeps configuration from
+   * before. This implementation also stops movement.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * NOT cause the controller to move to its last set target.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * NOT cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * This implementation does nothing because the API always requires the starting position to be
+   * specified.
+   */
+  void tarePosition() override;
+
+  /**
+   * This implementation does nothing because the maximum velocity is configured using
+   * PathfinderLimits elsewhere.
+   *
+   * @param imaxVelocity Ignored.
+   */
+  void setMaxVelocity(std::int32_t imaxVelocity) override;
+
+  /**
+   * Starts the internal thread. This should not be called by normal users. This method is called
+   * by the `AsyncMotionProfileControllerBuilder` when making a new instance of this class.
+   */
+  void startThread();
+
+  /**
+   * @return The underlying thread handle.
+   */
+  CrossplatformThread *getThread() const;
+
+  /**
+   * Saves a generated path to a file. Paths are stored as `<ipathId>.csv`. An SD card
+   * must be inserted into the brain and the directory must exist. `idirectory` can be prefixed with
+   * `/usd/`, but it this is not required.
+   *
+   * @param idirectory The directory to store the path file in
+   * @param ipathId The path ID of the generated path
+   */
+  void storePath(const std::string &idirectory, const std::string &ipathId);
+
+  /**
+   * Loads a path from a directory on the SD card containing a path CSV file. `/usd/` is
+   * automatically prepended to `idirectory` if it is not specified.
+   *
+   * @param idirectory The directory that the path files are stored in
+   * @param ipathId The path ID that the paths are stored under (and will be loaded into)
+   */
+  void loadPath(const std::string &idirectory, const std::string &ipathId);
+
+  /**
+   * Attempts to remove a path without stopping execution. If that fails, disables the controller
+   * and removes the path.
+   *
+   * @param ipathId The path ID that will be removed
+   */
+  void forceRemovePath(const std::string &ipathId);
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
+  PathfinderLimits limits;
+  std::shared_ptr<ChassisModel> model;
+  ChassisScales scales;
+  AbstractMotor::GearsetRatioPair pair;
+  TimeUtil timeUtil;
+
+  // This must be locked when accessing the current path
+  CrossplatformMutex currentPathMutex;
+
+  std::string currentPath{""};
+  std::atomic_bool isRunning{false};
+  std::atomic_int direction{1};
+  std::atomic_bool mirrored{false};
+  std::atomic_bool disabled{false};
+  std::atomic_bool dtorCalled{false};
+  CrossplatformThread *task{nullptr};
+
+  static void trampoline(void *context);
+  void loop();
+
+  /**
+   * Follow the supplied path. Must follow the disabled lifecycle.
+   */
+  virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
+                                 std::unique_ptr<AbstractRate> rate);
+
+  /**
+   * Converts linear chassis speed to rotational motor speed.
+   *
+   * @param linear chassis frame speed
+   * @return motor frame speed
+   */
+  QAngularSpeed convertLinearToRotational(QSpeed linear) const;
+
+  std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
+                                  const std::string &ipathId,
+                                  int length);
+
+  /**
+   * Joins and escapes a directory and file name
+   *
+   * @param directory The directory path, separated by forward slashes (/) and with or without a
+   * trailing slash
+   * @param filename The file name in the directory
+   * @return the fully qualified and legal path name
+   */
+  static std::string makeFilePath(const std::string &directory, const std::string &filename);
+
+  void internalStorePath(std::ostream &file, const std::string &ipathId);
+  void internalLoadPath(std::istream &file, const std::string &ipathId);
+  void internalLoadPathfinderPath(std::istream &leftFile,
+                                  std::istream &rightFile,
+                                  const std::string &ipathId);
+
+  static constexpr double DT = 0.01;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncPosIntegratedController.hpp

@@ -0,0 +1,145 @@
+/*
+ * 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 "okapi/api/control/async/asyncPositionController.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+
+namespace okapi {
+/**
+ * Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
+ * units the motor is in.
+ */
+class AsyncPosIntegratedController : public AsyncPositionController<double, double> {
+  public:
+  /**
+   * Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
+   * whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
+   * zero.
+   *
+   * @param imotor The motor to control.
+   * @param ipair The gearset.
+   * @param imaxVelocity The maximum velocity after gearing.
+   * @param itimeUtil The TimeUtil.
+   * @param ilogger The logger this instance will log to.
+   */
+  AsyncPosIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
+                               const AbstractMotor::GearsetRatioPair &ipair,
+                               std::int32_t imaxVelocity,
+                               const TimeUtil &itimeUtil,
+                               const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Sets the target for the controller.
+   */
+  void setTarget(double itarget) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() override;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  double getProcessValue() const override;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * Blocks the current task until the controller has settled. Determining what settling means is
+   * implementation-dependent.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be [-1, 1].
+   *
+   * @param ivalue the controller's output in the range [-1, 1]
+   */
+  void controllerSet(double ivalue) override;
+
+  /**
+   * Sets the "absolute" zero position of the controller to its current position.
+   */
+  void tarePosition() override;
+
+  /**
+   * Sets a new maximum velocity in motor RPM [0-600].
+   *
+   * @param imaxVelocity The new maximum velocity in motor RPM [0-600].
+   */
+  void setMaxVelocity(std::int32_t imaxVelocity) override;
+
+  /**
+   * Stops the motor mid-movement. Does not change the last set target.
+   */
+  virtual void stop();
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  TimeUtil timeUtil;
+  std::shared_ptr<AbstractMotor> motor;
+  AbstractMotor::GearsetRatioPair pair;
+  std::int32_t maxVelocity;
+  double lastTarget{0};
+  double offset{0};
+  bool controllerIsDisabled{false};
+  bool hasFirstTarget{false};
+  std::unique_ptr<SettledUtil> settledUtil;
+
+  /**
+   * Resumes moving after the controller is reset. Should not cause movement if the controller is
+   * turned off, reset, and turned back on.
+   */
+  virtual void resumeMovement();
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncPosPidController.hpp

@@ -0,0 +1,100 @@
+/*
+ * 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 "okapi/api/control/async/asyncPositionController.hpp"
+#include "okapi/api/control/async/asyncWrapper.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativePosPidController.hpp"
+#include "okapi/api/control/offsettableControllerInput.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+class AsyncPosPIDController : public AsyncWrapper<double, double>,
+                              public AsyncPositionController<double, double> {
+  public:
+  /**
+   * An async position PID controller.
+   *
+   * @param iinput The controller input. Will be turned into an OffsettableControllerInput.
+   * @param ioutput The controller output.
+   * @param itimeUtil The TimeUtil.
+   * @param ikP The proportional gain.
+   * @param ikI The integral gain.
+   * @param ikD The derivative gain.
+   * @param ikBias The controller bias.
+   * @param iratio Any external gear ratio.
+   * @param iderivativeFilter The derivative filter.
+   */
+  AsyncPosPIDController(
+    const std::shared_ptr<ControllerInput<double>> &iinput,
+    const std::shared_ptr<ControllerOutput<double>> &ioutput,
+    const TimeUtil &itimeUtil,
+    double ikP,
+    double ikI,
+    double ikD,
+    double ikBias = 0,
+    double iratio = 1,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * An async position PID controller.
+   *
+   * @param iinput The controller input.
+   * @param ioutput The controller output.
+   * @param itimeUtil The TimeUtil.
+   * @param ikP The proportional gain.
+   * @param ikI The integral gain.
+   * @param ikD The derivative gain.
+   * @param ikBias The controller bias.
+   * @param iratio Any external gear ratio.
+   * @param iderivativeFilter The derivative filter.
+   */
+  AsyncPosPIDController(
+    const std::shared_ptr<OffsetableControllerInput> &iinput,
+    const std::shared_ptr<ControllerOutput<double>> &ioutput,
+    const TimeUtil &itimeUtil,
+    double ikP,
+    double ikI,
+    double ikD,
+    double ikBias = 0,
+    double iratio = 1,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Sets the "absolute" zero position of the controller to its current position.
+   */
+  void tarePosition() override;
+
+  /**
+   * This implementation does not respect the maximum velocity.
+   *
+   * @param imaxVelocity Ignored.
+   */
+  void setMaxVelocity(std::int32_t imaxVelocity) override;
+
+  /**
+   * Set controller gains.
+   *
+   * @param igains The new gains.
+   */
+  void setGains(const IterativePosPIDController::Gains &igains);
+
+  /**
+   * Gets the current gains.
+   *
+   * @return The current gains.
+   */
+  IterativePosPIDController::Gains getGains() const;
+
+  protected:
+  std::shared_ptr<OffsetableControllerInput> offsettableInput;
+  std::shared_ptr<IterativePosPIDController> internalController;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncPositionController.hpp

@@ -0,0 +1,28 @@
+/*
+ * 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 "okapi/api/control/async/asyncController.hpp"
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output>
+class AsyncPositionController : virtual public AsyncController<Input, Output> {
+  public:
+  /**
+   * Sets the "absolute" zero position of the controller to its current position.
+   */
+  virtual void tarePosition() = 0;
+
+  /**
+   * Sets a new maximum velocity (typically motor RPM [0-600]). The interpretation of the units
+   * of this velocity and whether it will be respected is implementation-dependent.
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  virtual void setMaxVelocity(std::int32_t imaxVelocity) = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncVelIntegratedController.hpp

@@ -0,0 +1,124 @@
+/*
+ * 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 "okapi/api/control/async/asyncVelocityController.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+/**
+ * Closed-loop controller that uses the V5 motor's onboard control to move. Input units are whatever
+ * units the motor is in.
+ */
+class AsyncVelIntegratedController : public AsyncVelocityController<double, double> {
+  public:
+  /**
+   * Closed-loop controller that uses the V5 motor's onboard control to move. Input units are
+   * whatever units the motor is in. Throws a std::invalid_argument exception if the gear ratio is
+   * zero.
+   *
+   * @param imotor The motor to control.
+   * @param ipair The gearset.
+   * @param imaxVelocity The maximum velocity after gearing.
+   * @param itimeUtil The TimeUtil.
+   * @param ilogger The logger this instance will log to.
+   */
+  AsyncVelIntegratedController(const std::shared_ptr<AbstractMotor> &imotor,
+                               const AbstractMotor::GearsetRatioPair &ipair,
+                               std::int32_t imaxVelocity,
+                               const TimeUtil &itimeUtil,
+                               const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Sets the target for the controller.
+   */
+  void setTarget(double itarget) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() override;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  double getProcessValue() const override;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * Blocks the current task until the controller has settled. Determining what settling means is
+   * implementation-dependent.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be [-1, 1].
+   *
+   * @param ivalue the controller's output in the range [-1, 1]
+   */
+  void controllerSet(double ivalue) override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  TimeUtil timeUtil;
+  std::shared_ptr<AbstractMotor> motor;
+  AbstractMotor::GearsetRatioPair pair;
+  std::int32_t maxVelocity;
+  double lastTarget = 0;
+  bool controllerIsDisabled = false;
+  bool hasFirstTarget = false;
+  std::unique_ptr<SettledUtil> settledUtil;
+
+  virtual void resumeMovement();
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncVelPidController.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 "okapi/api/control/async/asyncVelocityController.hpp"
+#include "okapi/api/control/async/asyncWrapper.hpp"
+#include "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativeVelPidController.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+class AsyncVelPIDController : public AsyncWrapper<double, double>,
+                              public AsyncVelocityController<double, double> {
+  public:
+  /**
+   * An async velocity PID controller.
+   *
+   * @param iinput The controller input.
+   * @param ioutput The controller output.
+   * @param itimeUtil The TimeUtil.
+   * @param ikP The proportional gain.
+   * @param ikD The derivative gain.
+   * @param ikF The feed-forward gain.
+   * @param ikSF A feed-forward gain to counteract static friction.
+   * @param ivelMath The VelMath used for calculating velocity.
+   * @param iratio Any external gear ratio.
+   * @param iderivativeFilter The derivative filter.
+   */
+  AsyncVelPIDController(
+    const std::shared_ptr<ControllerInput<double>> &iinput,
+    const std::shared_ptr<ControllerOutput<double>> &ioutput,
+    const TimeUtil &itimeUtil,
+    double ikP,
+    double ikD,
+    double ikF,
+    double ikSF,
+    std::unique_ptr<VelMath> ivelMath,
+    double iratio = 1,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Set controller gains.
+   *
+   * @param igains The new gains.
+   */
+  void setGains(const IterativeVelPIDController::Gains &igains);
+
+  /**
+   * Gets the current gains.
+   *
+   * @return The current gains.
+   */
+  IterativeVelPIDController::Gains getGains() const;
+
+  protected:
+  std::shared_ptr<IterativeVelPIDController> internalController;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncVelocityController.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 "okapi/api/control/async/asyncController.hpp"
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output>
+class AsyncVelocityController : virtual public AsyncController<Input, Output> {};
+} // namespace okapi

SerialTest/include/okapi/api/control/async/asyncWrapper.hpp

@@ -0,0 +1,287 @@
+/*
+ * 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 "okapi/api/control/async/asyncController.hpp"
+#include "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/control/iterative/iterativeController.hpp"
+#include "okapi/api/control/util/settledUtil.hpp"
+#include "okapi/api/coreProsAPI.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/mathUtil.hpp"
+#include "okapi/api/util/supplier.hpp"
+#include <atomic>
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output>
+class AsyncWrapper : virtual public AsyncController<Input, Output> {
+  public:
+  /**
+   * A wrapper class that transforms an `IterativeController` into an `AsyncController` by running
+   * it in another task. The input controller will act like an `AsyncController`.
+   *
+   * @param iinput controller input, passed to the `IterativeController`
+   * @param ioutput controller output, written to from the `IterativeController`
+   * @param icontroller the controller to use
+   * @param irateSupplier used for rates used in the main loop and in `waitUntilSettled`
+   * @param iratio Any external gear ratio.
+   * @param ilogger The logger this instance will log to.
+   */
+  AsyncWrapper(const std::shared_ptr<ControllerInput<Input>> &iinput,
+               const std::shared_ptr<ControllerOutput<Output>> &ioutput,
+               const std::shared_ptr<IterativeController<Input, Output>> &icontroller,
+               const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
+               const double iratio = 1,
+               std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger())
+    : logger(std::move(ilogger)),
+      rateSupplier(irateSupplier),
+      input(iinput),
+      output(ioutput),
+      controller(icontroller),
+      ratio(iratio) {
+  }
+
+  AsyncWrapper(AsyncWrapper<Input, Output> &&other) = delete;
+
+  AsyncWrapper<Input, Output> &operator=(AsyncWrapper<Input, Output> &&other) = delete;
+
+  ~AsyncWrapper() override {
+    dtorCalled.store(true, std::memory_order_release);
+    delete task;
+  }
+
+  /**
+   * Sets the target for the controller.
+   */
+  void setTarget(const Input itarget) override {
+    LOG_INFO("AsyncWrapper: Set target to " + std::to_string(itarget));
+    hasFirstTarget = true;
+    controller->setTarget(itarget * ratio);
+    lastTarget = itarget;
+  }
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller.
+   *
+   * @param ivalue the controller's output
+   */
+  void controllerSet(const Input ivalue) override {
+    controller->controllerSet(ivalue);
+  }
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  Input getTarget() override {
+    return controller->getTarget();
+  }
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  Input getProcessValue() const override {
+    return controller->getProcessValue();
+  }
+
+  /**
+   * Returns the last calculated output of the controller.
+   */
+  Output getOutput() const {
+    return controller->getOutput();
+  }
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  Output getError() const override {
+    return controller->getError();
+  }
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override {
+    return isDisabled() || controller->isSettled();
+  }
+
+  /**
+   * Set time between loops.
+   *
+   * @param isampleTime time between loops
+   */
+  void setSampleTime(const QTime &isampleTime) {
+    controller->setSampleTime(isampleTime);
+  }
+
+  /**
+   * Set controller output bounds.
+   *
+   * @param imax max output
+   * @param imin min output
+   */
+  void setOutputLimits(const Output imax, const Output imin) {
+    controller->setOutputLimits(imax, imin);
+  }
+
+  /**
+   * Sets the (soft) limits for the target range that controllerSet() scales into. The target
+   * computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
+   *
+   * @param itargetMax The new max target for controllerSet().
+   * @param itargetMin The new min target for controllerSet().
+   */
+  void setControllerSetTargetLimits(double itargetMax, double itargetMin) {
+    controller->setControllerSetTargetLimits(itargetMax, itargetMin);
+  }
+
+  /**
+   * Get the upper output bound.
+   *
+   * @return  the upper output bound
+   */
+  Output getMaxOutput() {
+    return controller->getMaxOutput();
+  }
+
+  /**
+   * Get the lower output bound.
+   *
+   * @return the lower output bound
+   */
+  Output getMinOutput() {
+    return controller->getMinOutput();
+  }
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override {
+    LOG_INFO_S("AsyncWrapper: Reset");
+    controller->reset();
+    hasFirstTarget = false;
+  }
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override {
+    LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(!controller->isDisabled()));
+    controller->flipDisable();
+    resumeMovement();
+  }
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(const bool iisDisabled) override {
+    LOG_INFO("AsyncWrapper: flipDisable " + std::to_string(iisDisabled));
+    controller->flipDisable(iisDisabled);
+    resumeMovement();
+  }
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override {
+    return controller->isDisabled();
+  }
+
+  /**
+   * Blocks the current task until the controller has settled. Determining what settling means is
+   * implementation-dependent.
+   */
+  void waitUntilSettled() override {
+    LOG_INFO_S("AsyncWrapper: Waiting to settle");
+
+    auto rate = rateSupplier.get();
+    while (!isSettled()) {
+      rate->delayUntil(motorUpdateRate);
+    }
+
+    LOG_INFO_S("AsyncWrapper: Done waiting to settle");
+  }
+
+  /**
+   * Starts the internal thread. This should not be called by normal users. This method is called
+   * by the AsyncControllerFactory when making a new instance of this class.
+   */
+  void startThread() {
+    if (!task) {
+      task = new CrossplatformThread(trampoline, this, "AsyncWrapper");
+    }
+  }
+
+  /**
+   * Returns the underlying thread handle.
+   *
+   * @return The underlying thread handle.
+   */
+  CrossplatformThread *getThread() const {
+    return task;
+  }
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
+  std::shared_ptr<ControllerInput<Input>> input;
+  std::shared_ptr<ControllerOutput<Output>> output;
+  std::shared_ptr<IterativeController<Input, Output>> controller;
+  bool hasFirstTarget{false};
+  Input lastTarget;
+  double ratio;
+  std::atomic_bool dtorCalled{false};
+  CrossplatformThread *task{nullptr};
+
+  static void trampoline(void *context) {
+    if (context) {
+      static_cast<AsyncWrapper *>(context)->loop();
+    }
+  }
+
+  void loop() {
+    auto rate = rateSupplier.get();
+    while (!dtorCalled.load(std::memory_order_acquire) && !task->notifyTake(0)) {
+      if (!isDisabled()) {
+        output->controllerSet(controller->step(input->controllerGet()));
+      }
+
+      rate->delayUntil(controller->getSampleTime());
+    }
+  }
+
+  /**
+   * Resumes moving after the controller is reset. Should not cause movement if the controller is
+   * turned off, reset, and turned back on.
+   */
+  virtual void resumeMovement() {
+    if (isDisabled()) {
+      // This will grab the output *when disabled*
+      output->controllerSet(controller->getOutput());
+    } else {
+      if (hasFirstTarget) {
+        setTarget(lastTarget);
+      }
+    }
+  }
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/closedLoopController.hpp

@@ -0,0 +1,86 @@
+/*
+ * 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 "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/units/QTime.hpp"
+
+namespace okapi {
+/**
+ * An abstract closed-loop controller.
+ *
+ * @tparam Input The target/input type.
+ * @tparam Output The error/output type.
+ */
+template <typename Input, typename Output>
+class ClosedLoopController : public ControllerOutput<Input> {
+  public:
+  virtual ~ClosedLoopController() = default;
+
+  /**
+   * Sets the target for the controller.
+   *
+   * @param itarget the new target
+   */
+  virtual void setTarget(Input itarget) = 0;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  virtual Input getTarget() = 0;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  virtual Input getProcessValue() const = 0;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   *
+   * @return the last error
+   */
+  virtual Output getError() const = 0;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return `true`.
+   *
+   * @return whether the controller is settled
+   */
+  virtual bool isSettled() = 0;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  virtual void reset() = 0;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  virtual void flipDisable() = 0;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  virtual void flipDisable(bool iisDisabled) = 0;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  virtual bool isDisabled() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/controllerInput.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
+
+namespace okapi {
+template <typename T> class ControllerInput {
+  public:
+  /**
+   * Get the sensor value for use in a control loop. This method might be automatically called in
+   * another thread by the controller.
+   *
+   * @return the current sensor value, or ``PROS_ERR`` on a failure.
+   */
+  virtual T controllerGet() = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/controllerOutput.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
+
+namespace okapi {
+template <typename T> class ControllerOutput {
+  public:
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be `[-1, 1]`.
+   *
+   * @param ivalue the controller's output in the range `[-1, 1]`
+   */
+  virtual void controllerSet(T ivalue) = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativeController.hpp

@@ -0,0 +1,79 @@
+/*
+ * 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 "okapi/api/control/closedLoopController.hpp"
+#include "okapi/api/units/QTime.hpp"
+
+namespace okapi {
+/**
+ * Closed-loop controller that steps iteratively using the step method below.
+ *
+ * `ControllerOutput::controllerSet()` should set the controller's target to the input scaled by
+ * the output bounds.
+ */
+template <typename Input, typename Output>
+class IterativeController : public ClosedLoopController<Input, Output> {
+  public:
+  /**
+   * Do one iteration of the controller.
+   *
+   * @param ireading A new measurement.
+   * @return The controller output.
+   */
+  virtual Output step(Input ireading) = 0;
+
+  /**
+   * Returns the last calculated output of the controller.
+   */
+  virtual Output getOutput() const = 0;
+
+  /**
+   * Set controller output bounds.
+   *
+   * @param imax max output
+   * @param imin min output
+   */
+  virtual void setOutputLimits(Output imax, Output imin) = 0;
+
+  /**
+   * Sets the (soft) limits for the target range that controllerSet() scales into. The target
+   * computed by `controllerSet()` is scaled into the range `[-itargetMin, itargetMax]`.
+   *
+   * @param itargetMax The new max target for `controllerSet()`.
+   * @param itargetMin The new min target for `controllerSet()`.
+   */
+  virtual void setControllerSetTargetLimits(Output itargetMax, Output itargetMin) = 0;
+
+  /**
+   * Get the upper output bound.
+   *
+   * @return  the upper output bound
+   */
+  virtual Output getMaxOutput() = 0;
+
+  /**
+   * Get the lower output bound.
+   *
+   * @return the lower output bound
+   */
+  virtual Output getMinOutput() = 0;
+
+  /**
+   * Set time between loops.
+   *
+   * @param isampleTime time between loops
+   */
+  virtual void setSampleTime(QTime isampleTime) = 0;
+
+  /**
+   * Get the last set sample time.
+   *
+   * @return sample time
+   */
+  virtual QTime getSampleTime() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativeMotorVelocityController.hpp

@@ -0,0 +1,150 @@
+/*
+ * 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 "okapi/api/control/iterative/iterativeVelocityController.hpp"
+#include "okapi/api/device/motor/abstractMotor.hpp"
+#include <array>
+#include <memory>
+
+namespace okapi {
+class IterativeMotorVelocityController : public IterativeVelocityController<double, double> {
+  public:
+  /**
+   * Velocity controller that automatically writes to the motor.
+   */
+  IterativeMotorVelocityController(
+    const std::shared_ptr<AbstractMotor> &imotor,
+    const std::shared_ptr<IterativeVelocityController<double, double>> &icontroller);
+
+  /**
+   * Do one iteration of the controller.
+   *
+   * @param inewReading new measurement
+   * @return controller output
+   */
+  double step(double ireading) override;
+
+  /**
+   * Sets the target for the controller.
+   */
+  void setTarget(double itarget) override;
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be `[-1, 1]`.
+   *
+   * @param ivalue the controller's output in the range `[-1, 1]`
+   */
+  void controllerSet(double ivalue) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() override;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  double getProcessValue() const override;
+
+  /**
+   * Returns the last calculated output of the controller.
+   */
+  double getOutput() const override;
+
+  /**
+   * Get the upper output bound.
+   *
+   * @return  the upper output bound
+   */
+  double getMaxOutput() override;
+
+  /**
+   * Get the lower output bound.
+   *
+   * @return the lower output bound
+   */
+  double getMinOutput() override;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Set time between loops in ms.
+   *
+   * @param isampleTime time between loops in ms
+   */
+  void setSampleTime(QTime isampleTime) override;
+
+  /**
+   * Set controller output bounds.
+   *
+   * @param imax max output
+   * @param imin min output
+   */
+  void setOutputLimits(double imax, double imin) override;
+
+  /**
+   * Sets the (soft) limits for the target range that controllerSet() scales into. The target
+   * computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
+   *
+   * @param itargetMax The new max target for controllerSet().
+   * @param itargetMin The new min target for controllerSet().
+   */
+  void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * Get the last set sample time.
+   *
+   * @return sample time
+   */
+  QTime getSampleTime() const override;
+
+  protected:
+  std::shared_ptr<AbstractMotor> motor;
+  std::shared_ptr<IterativeVelocityController<double, double>> controller;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativePosPidController.hpp

@@ -0,0 +1,276 @@
+/*
+ * Based on the Arduino PID controller: https://github.com/br3ttb/Arduino-PID-Library
+ *
+ * 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 "okapi/api/control/iterative/iterativePositionController.hpp"
+#include "okapi/api/control/util/settledUtil.hpp"
+#include "okapi/api/filter/filter.hpp"
+#include "okapi/api/filter/passthroughFilter.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <limits>
+#include <memory>
+
+namespace okapi {
+class IterativePosPIDController : public IterativePositionController<double, double> {
+  public:
+  struct Gains {
+    double kP{0};
+    double kI{0};
+    double kD{0};
+    double kBias{0};
+
+    bool operator==(const Gains &rhs) const;
+    bool operator!=(const Gains &rhs) const;
+  };
+
+  /**
+   * Position PID controller.
+   *
+   * @param ikP the proportional gain
+   * @param ikI the integration gain
+   * @param ikD the derivative gain
+   * @param ikBias the controller bias
+   * @param itimeUtil see TimeUtil docs
+   * @param iderivativeFilter a filter for filtering the derivative term
+   * @param ilogger The logger this instance will log to.
+   */
+  IterativePosPIDController(
+    double ikP,
+    double ikI,
+    double ikD,
+    double ikBias,
+    const TimeUtil &itimeUtil,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Position PID controller.
+   *
+   * @param igains the controller gains
+   * @param itimeUtil see TimeUtil docs
+   * @param iderivativeFilter a filter for filtering the derivative term
+   */
+  IterativePosPIDController(
+    const Gains &igains,
+    const TimeUtil &itimeUtil,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Do one iteration of the controller. Returns the reading in the range [-1, 1] unless the
+   * bounds have been changed with setOutputLimits().
+   *
+   * @param inewReading new measurement
+   * @return controller output
+   */
+  double step(double inewReading) override;
+
+  /**
+   * Sets the target for the controller.
+   *
+   * @param itarget new target position
+   */
+  void setTarget(double itarget) override;
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be [-1, 1].
+   *
+   * @param ivalue the controller's output in the range [-1, 1]
+   */
+  void controllerSet(double ivalue) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() const;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  double getProcessValue() const override;
+
+  /**
+   * Returns the last calculated output of the controller. Output is in the range [-1, 1]
+   * unless the bounds have been changed with setOutputLimits().
+   */
+  double getOutput() const override;
+
+  /**
+   * Get the upper output bound.
+   *
+   * @return  the upper output bound
+   */
+  double getMaxOutput() override;
+
+  /**
+   * Get the lower output bound.
+   *
+   * @return the lower output bound
+   */
+  double getMinOutput() override;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Set time between loops in ms.
+   *
+   * @param isampleTime time between loops
+   */
+  void setSampleTime(QTime isampleTime) override;
+
+  /**
+   * Set controller output bounds. Default bounds are [-1, 1].
+   *
+   * @param imax max output
+   * @param imin min output
+   */
+  void setOutputLimits(double imax, double imin) override;
+
+  /**
+   * Sets the (soft) limits for the target range that controllerSet() scales into. The target
+   * computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
+   *
+   * @param itargetMax The new max target for controllerSet().
+   * @param itargetMin The new min target for controllerSet().
+   */
+  void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * Get the last set sample time.
+   *
+   * @return sample time
+   */
+  QTime getSampleTime() const override;
+
+  /**
+   * Set integrator bounds. Default bounds are [-1, 1].
+   *
+   * @param imax max integrator value
+   * @param imin min integrator value
+   */
+  virtual void setIntegralLimits(double imax, double imin);
+
+  /**
+   * Set the error sum bounds. Default bounds are [0, std::numeric_limits<double>::max()]. Error
+   * will only be added to the integral term when its absolute value is between these bounds of
+   * either side of the target.
+   *
+   * @param imax max error value that will be summed
+   * @param imin min error value that will be summed
+   */
+  virtual void setErrorSumLimits(double imax, double imin);
+
+  /**
+   * Set whether the integrator should be reset when error is 0 or changes sign.
+   *
+   * @param iresetOnZero true to reset
+   */
+  virtual void setIntegratorReset(bool iresetOnZero);
+
+  /**
+   * Set controller gains.
+   *
+   * @param igains The new gains.
+   */
+  virtual void setGains(const Gains &igains);
+
+  /**
+   * Gets the current gains.
+   *
+   * @return The current gains.
+   */
+  Gains getGains() const;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  double kP, kI, kD, kBias;
+  QTime sampleTime{10_ms};
+  double target{0};
+  double lastReading{0};
+  double error{0};
+  double lastError{0};
+  std::unique_ptr<Filter> derivativeFilter;
+
+  // Integral bounds
+  double integral{0};
+  double integralMax{1};
+  double integralMin{-1};
+
+  // Error will only be added to the integral term within these bounds on either side of the target
+  double errorSumMin{0};
+  double errorSumMax{std::numeric_limits<double>::max()};
+
+  double derivative{0};
+
+  // Output bounds
+  double output{0};
+  double outputMax{1};
+  double outputMin{-1};
+  double controllerSetTargetMax{1};
+  double controllerSetTargetMin{-1};
+
+  // Reset the integrated when the controller crosses 0 or not
+  bool shouldResetOnCross{true};
+
+  bool controllerIsDisabled{false};
+
+  std::unique_ptr<AbstractTimer> loopDtTimer;
+  std::unique_ptr<SettledUtil> settledUtil;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativePositionController.hpp

@@ -0,0 +1,13 @@
+/*
+ * 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 "okapi/api/control/iterative/iterativeController.hpp"
+
+namespace okapi {
+template <typename Input, typename Output>
+class IterativePositionController : public IterativeController<Input, Output> {};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativeVelPidController.hpp

@@ -0,0 +1,255 @@
+/*
+ * 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 "okapi/api/control/iterative/iterativeVelocityController.hpp"
+#include "okapi/api/control/util/settledUtil.hpp"
+#include "okapi/api/filter/passthroughFilter.hpp"
+#include "okapi/api/filter/velMath.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+
+namespace okapi {
+class IterativeVelPIDController : public IterativeVelocityController<double, double> {
+  public:
+  struct Gains {
+    double kP{0};
+    double kD{0};
+    double kF{0};
+    double kSF{0};
+
+    bool operator==(const Gains &rhs) const;
+    bool operator!=(const Gains &rhs) const;
+  };
+
+  /**
+   * Velocity PD controller.
+   *
+   * @param ikP the proportional gain
+   * @param ikD the derivative gain
+   * @param ikF the feed-forward gain
+   * @param ikSF a feed-forward gain to counteract static friction
+   * @param ivelMath The VelMath used for calculating velocity.
+   * @param itimeUtil see TimeUtil docs
+   * @param iderivativeFilter a filter for filtering the derivative term
+   * @param ilogger The logger this instance will log to.
+   */
+  IterativeVelPIDController(
+    double ikP,
+    double ikD,
+    double ikF,
+    double ikSF,
+    std::unique_ptr<VelMath> ivelMath,
+    const TimeUtil &itimeUtil,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Velocity PD controller.
+   *
+   * @param igains The controller gains.
+   * @param ivelMath The VelMath used for calculating velocity.
+   * @param itimeUtil see TimeUtil docs
+   * @param iderivativeFilter a filter for filtering the derivative term
+   * @param ilogger The logger this instance will log to.
+   */
+  IterativeVelPIDController(
+    const Gains &igains,
+    std::unique_ptr<VelMath> ivelMath,
+    const TimeUtil &itimeUtil,
+    std::unique_ptr<Filter> iderivativeFilter = std::make_unique<PassthroughFilter>(),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * Do one iteration of the controller. Returns the reading in the range [-1, 1] unless the
+   * bounds have been changed with setOutputLimits().
+   *
+   * @param inewReading new measurement
+   * @return controller output
+   */
+  double step(double inewReading) override;
+
+  /**
+   * Sets the target for the controller.
+   *
+   * @param itarget new target velocity
+   */
+  void setTarget(double itarget) override;
+
+  /**
+   * Writes the value of the controller output. This method might be automatically called in another
+   * thread by the controller. The range of input values is expected to be [-1, 1].
+   *
+   * @param ivalue the controller's output in the range [-1, 1]
+   */
+  void controllerSet(double ivalue) override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() override;
+
+  /**
+   * Gets the last set target, or the default target if none was set.
+   *
+   * @return the last target
+   */
+  double getTarget() const;
+
+  /**
+   * @return The most recent value of the process variable.
+   */
+  double getProcessValue() const override;
+
+  /**
+   * Returns the last calculated output of the controller.
+   */
+  double getOutput() const override;
+
+  /**
+   * Get the upper output bound.
+   *
+   * @return  the upper output bound
+   */
+  double getMaxOutput() override;
+
+  /**
+   * Get the lower output bound.
+   *
+   * @return the lower output bound
+   */
+  double getMinOutput() override;
+
+  /**
+   * Returns the last error of the controller. Does not update when disabled.
+   */
+  double getError() const override;
+
+  /**
+   * Returns whether the controller has settled at the target. Determining what settling means is
+   * implementation-dependent.
+   *
+   * If the controller is disabled, this method must return true.
+   *
+   * @return whether the controller is settled
+   */
+  bool isSettled() override;
+
+  /**
+   * Set time between loops in ms.
+   *
+   * @param isampleTime time between loops
+   */
+  void setSampleTime(QTime isampleTime) override;
+
+  /**
+   * Set controller output bounds. Default bounds are [-1, 1].
+   *
+   * @param imax max output
+   * @param imin min output
+   */
+  void setOutputLimits(double imax, double imin) override;
+
+  /**
+   * Sets the (soft) limits for the target range that controllerSet() scales into. The target
+   * computed by controllerSet() is scaled into the range [-itargetMin, itargetMax].
+   *
+   * @param itargetMax The new max target for controllerSet().
+   * @param itargetMin The new min target for controllerSet().
+   */
+  void setControllerSetTargetLimits(double itargetMax, double itargetMin) override;
+
+  /**
+   * Resets the controller's internal state so it is similar to when it was first initialized, while
+   * keeping any user-configured information.
+   */
+  void reset() override;
+
+  /**
+   * Changes whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   */
+  void flipDisable() override;
+
+  /**
+   * Sets whether the controller is off or on. Turning the controller on after it was off will
+   * cause the controller to move to its last set target, unless it was reset in that time.
+   *
+   * @param iisDisabled whether the controller is disabled
+   */
+  void flipDisable(bool iisDisabled) override;
+
+  /**
+   * Returns whether the controller is currently disabled.
+   *
+   * @return whether the controller is currently disabled
+   */
+  bool isDisabled() const override;
+
+  /**
+   * Get the last set sample time.
+   *
+   * @return sample time
+   */
+  QTime getSampleTime() const override;
+
+  /**
+   * Do one iteration of velocity calculation.
+   *
+   * @param inewReading new measurement
+   * @return filtered velocity
+   */
+  virtual QAngularSpeed stepVel(double inewReading);
+
+  /**
+   * Set controller gains.
+   *
+   * @param igains The new gains.
+   */
+  virtual void setGains(const Gains &igains);
+
+  /**
+   * Gets the current gains.
+   *
+   * @return The current gains.
+   */
+  Gains getGains() const;
+
+  /**
+   * Sets the number of encoder ticks per revolution. Default is 1800.
+   *
+   * @param tpr number of measured units per revolution
+   */
+  virtual void setTicksPerRev(double tpr);
+
+  /**
+   * Returns the current velocity.
+   */
+  virtual QAngularSpeed getVel() const;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  double kP, kD, kF, kSF;
+  QTime sampleTime{10_ms};
+  double error{0};
+  double derivative{0};
+  double target{0};
+  double outputSum{0};
+  double output{0};
+  double outputMax{1};
+  double outputMin{-1};
+  double controllerSetTargetMax{1};
+  double controllerSetTargetMin{-1};
+  bool controllerIsDisabled{false};
+
+  std::unique_ptr<VelMath> velMath;
+  std::unique_ptr<Filter> derivativeFilter;
+  std::unique_ptr<AbstractTimer> loopDtTimer;
+  std::unique_ptr<SettledUtil> settledUtil;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/iterative/iterativeVelocityController.hpp

@@ -0,0 +1,13 @@
+/*
+ * 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 "okapi/api/control/iterative/iterativeController.hpp"
+
+namespace okapi {
+template <typename Input, typename Output>
+class IterativeVelocityController : public IterativeController<Input, Output> {};
+} // namespace okapi

SerialTest/include/okapi/api/control/offsettableControllerInput.hpp

@@ -0,0 +1,41 @@
+/*
+ * 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 "okapi/api/control/controllerInput.hpp"
+#include <memory>
+
+namespace okapi {
+class OffsetableControllerInput : public ControllerInput<double> {
+  public:
+  /**
+   * A ControllerInput which can be tared to change the zero position.
+   *
+   * @param iinput The ControllerInput to reference.
+   */
+  explicit OffsetableControllerInput(const std::shared_ptr<ControllerInput<double>> &iinput);
+
+  virtual ~OffsetableControllerInput();
+
+  /**
+   * Get the sensor value for use in a control loop. This method might be automatically called in
+   * another thread by the controller.
+   *
+   * @return the current sensor value, or PROS_ERR on a failure.
+   */
+  double controllerGet() override;
+
+  /**
+   * Sets the "absolute" zero position of this controller input to its current position. This does
+   * nothing if the underlying controller input returns PROS_ERR.
+   */
+  virtual void tarePosition();
+
+  protected:
+  std::shared_ptr<ControllerInput<double>> input;
+  double offset{0};
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/util/controllerRunner.hpp

@@ -0,0 +1,131 @@
+/*
+ * 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 "okapi/api/control/async/asyncController.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativeController.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output> class ControllerRunner {
+  public:
+  /**
+   * A utility class that runs a closed-loop controller.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param ilogger The logger this instance will log to.
+   */
+  explicit ControllerRunner(const TimeUtil &itimeUtil,
+                            const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger())
+    : logger(ilogger), rate(itimeUtil.getRate()) {
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget, AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget,
+                                 IterativeController<Input, Output> &icontroller,
+                                 ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      ioutput.controllerSet(icontroller.getOutput());
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  IterativeController<Input, Output> &icontroller,
+                                  ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      ioutput.controllerSet(icontroller.getOutput());
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::unique_ptr<AbstractRate> rate;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/util/flywheelSimulator.hpp

@@ -0,0 +1,156 @@
+/*
+ * 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>
+
+namespace okapi {
+class FlywheelSimulator {
+  public:
+  /**
+   * A simulator for an inverted pendulum. The center of mass of the system changes as the link
+   * rotates (by default, you can set a new torque function with setExternalTorqueFunction()).
+   */
+  explicit FlywheelSimulator(double imass = 0.01,
+                             double ilinkLen = 1,
+                             double imuStatic = 0.1,
+                             double imuDynamic = 0.9,
+                             double itimestep = 0.01);
+
+  virtual ~FlywheelSimulator();
+
+  /**
+   * Step the simulation by the timestep.
+   *
+   * @return the current angle
+   */
+  double step();
+
+  /**
+   * Step the simulation by the timestep.
+   *
+   * @param itorque new input torque
+   * @return the current angle
+   */
+  double step(double itorque);
+
+  /**
+   * Sets the torque function used to calculate the torque due to external forces. This torque gets
+   * summed with the input torque.
+   *
+   * For example, the default torque function has the torque due to gravity vary as the link swings:
+   * [](double angle, double mass, double linkLength) {
+   *   return (linkLength * std::cos(angle)) * (mass * -1 * gravity);
+   * }
+   *
+   * @param itorqueFunc the torque function. The return value is the torque due to external forces
+   */
+  void setExternalTorqueFunction(
+    std::function<double(double angle, double mass, double linkLength)> itorqueFunc);
+
+  /**
+   * Sets the input torque. The input will be bounded by the max torque.
+   *
+   * @param itorque new input torque
+   */
+  void setTorque(double itorque);
+
+  /**
+   * Sets the max torque. The input torque cannot exceed this maximum torque.
+   *
+   * @param imaxTorque new maximum torque
+   */
+  void setMaxTorque(double imaxTorque);
+
+  /**
+   * Sets the current angle.
+   *
+   * @param iangle new angle
+   **/
+  void setAngle(double iangle);
+
+  /**
+   * Sets the mass (kg).
+   *
+   * @param imass new mass
+   */
+  void setMass(double imass);
+
+  /**
+   * Sets the link length (m).
+   *
+   * @param ilinkLen new link length
+   */
+  void setLinkLength(double ilinkLen);
+
+  /**
+   * Sets the static friction (N*m).
+   *
+   * @param imuStatic new static friction
+   */
+  void setStaticFriction(double imuStatic);
+
+  /**
+   * Sets the dynamic friction (N*m).
+   *
+   * @param imuDynamic new dynamic friction
+   */
+  void setDynamicFriction(double imuDynamic);
+
+  /**
+   * Sets the timestep (sec).
+   *
+   * @param itimestep new timestep
+   */
+  void setTimestep(double itimestep);
+
+  /**
+   * Returns the current angle (angle in rad).
+   *
+   * @return the current angle
+   */
+  double getAngle() const;
+
+  /**
+   * Returns the current omgea (angular velocity in rad / sec).
+   *
+   * @return the current omega
+   */
+  double getOmega() const;
+
+  /**
+   * Returns the current acceleration (angular acceleration in rad / sec^2).
+   *
+   * @return the current acceleration
+   */
+  double getAcceleration() const;
+
+  /**
+   * Returns the maximum torque input.
+   *
+   * @return the max torque input
+   */
+  double getMaxTorque() const;
+
+  protected:
+  double inputTorque = 0;    // N*m
+  double maxTorque = 0.5649; // N*m
+  double angle = 0;          // rad
+  double omega = 0;          // rad / sec
+  double accel = 0;          // rad / sec^2
+  double mass;               // kg
+  double linkLen;            // m
+  double muStatic;           // N*m
+  double muDynamic;          // N*m
+  double timestep;           // sec
+  double I = 0;              // moment of inertia
+  std::function<double(double, double, double)> torqueFunc;
+
+  const double minTimestep = 0.000001; // 1 us
+
+  virtual double stepImpl();
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/util/pathfinderUtil.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 "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+
+namespace okapi {
+struct PathfinderPoint {
+  QLength x;    // X coordinate relative to the start of the movement
+  QLength y;    // Y coordinate relative to the start of the movement
+  QAngle theta; // Exit angle relative to the start of the movement
+};
+
+struct PathfinderLimits {
+  double maxVel;   // Maximum robot velocity in m/s
+  double maxAccel; // Maximum robot acceleration in m/s/s
+  double maxJerk;  // Maximum robot jerk in m/s/s/s
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/util/pidTuner.hpp

@@ -0,0 +1,80 @@
+/*
+ * 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 "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativePosPidController.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+#include <vector>
+
+namespace okapi {
+class PIDTuner {
+  public:
+  struct Output {
+    double kP, kI, kD;
+  };
+
+  PIDTuner(const std::shared_ptr<ControllerInput<double>> &iinput,
+           const std::shared_ptr<ControllerOutput<double>> &ioutput,
+           const TimeUtil &itimeUtil,
+           QTime itimeout,
+           std::int32_t igoal,
+           double ikPMin,
+           double ikPMax,
+           double ikIMin,
+           double ikIMax,
+           double ikDMin,
+           double ikDMax,
+           std::size_t inumIterations = 5,
+           std::size_t inumParticles = 16,
+           double ikSettle = 1,
+           double ikITAE = 2,
+           const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  virtual ~PIDTuner();
+
+  virtual Output autotune();
+
+  protected:
+  static constexpr double inertia = 0.5;   // Particle inertia
+  static constexpr double confSelf = 1.1;  // Self confidence
+  static constexpr double confSwarm = 1.2; // Particle swarm confidence
+  static constexpr int increment = 5;
+  static constexpr int divisor = 5;
+  static constexpr QTime loopDelta = 10_ms; // NOLINT
+
+  struct Particle {
+    double pos, vel, best;
+  };
+
+  struct ParticleSet {
+    Particle kP, kI, kD;
+    double bestError;
+  };
+
+  std::shared_ptr<Logger> logger;
+  TimeUtil timeUtil;
+  std::shared_ptr<ControllerInput<double>> input;
+  std::shared_ptr<ControllerOutput<double>> output;
+
+  const QTime timeout;
+  const std::int32_t goal;
+  const double kPMin;
+  const double kPMax;
+  const double kIMin;
+  const double kIMax;
+  const double kDMin;
+  const double kDMax;
+  const std::size_t numIterations;
+  const std::size_t numParticles;
+  const double kSettle;
+  const double kITAE;
+};
+} // namespace okapi

SerialTest/include/okapi/api/control/util/settledUtil.hpp

@@ -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/.
+ */
+#pragma once
+
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include <memory>
+
+namespace okapi {
+class SettledUtil {
+  public:
+  /**
+   * A utility class to determine if a control loop has settled based on error. A control loop is
+   * settled if the error is within `iatTargetError` and `iatTargetDerivative` for `iatTargetTime`.
+   *
+   * @param iatTargetTimer A timer used to track `iatTargetTime`.
+   * @param iatTargetError The minimum error to be considered settled.
+   * @param iatTargetDerivative The minimum error derivative to be considered settled.
+   * @param iatTargetTime The minimum time within atTargetError to be considered settled.
+   */
+  explicit SettledUtil(std::unique_ptr<AbstractTimer> iatTargetTimer,
+                       double iatTargetError = 50,
+                       double iatTargetDerivative = 5,
+                       QTime iatTargetTime = 250_ms);
+
+  virtual ~SettledUtil();
+
+  /**
+   * Returns whether the controller is settled.
+   *
+   * @param ierror The current error.
+   * @return Whether the controller is settled.
+   */
+  virtual bool isSettled(double ierror);
+
+  /**
+   * Resets the "at target" timer and clears the previous error.
+   */
+  virtual void reset();
+
+  protected:
+  double atTargetError = 50;
+  double atTargetDerivative = 5;
+  QTime atTargetTime = 250_ms;
+  std::unique_ptr<AbstractTimer> atTargetTimer;
+  double lastError = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/coreProsAPI.hpp

@@ -0,0 +1,131 @@
+/*
+ * 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 <cmath>
+#include <cstdbool>
+#include <cstddef>
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <functional>
+#include <sstream>
+
+#ifdef THREADS_STD
+#include <thread>
+#define CROSSPLATFORM_THREAD_T std::thread
+
+#include <mutex>
+#define CROSSPLATFORM_MUTEX_T std::mutex
+#else
+#include "api.h"
+#include "pros/apix.h"
+#define CROSSPLATFORM_THREAD_T pros::task_t
+#define CROSSPLATFORM_MUTEX_T pros::Mutex
+#endif
+
+#define NOT_INITIALIZE_TASK                                                                        \
+  (strcmp(pros::c::task_get_name(pros::c::task_get_current()), "User Initialization (PROS)") != 0)
+
+#define NOT_COMP_INITIALIZE_TASK                                                                   \
+  (strcmp(pros::c::task_get_name(pros::c::task_get_current()), "User Comp. Init. (PROS)") != 0)
+
+class CrossplatformThread {
+  public:
+#ifdef THREADS_STD
+  CrossplatformThread(void (*ptr)(void *),
+                      void *params,
+                      const char *const = "OkapiLibCrossplatformTask")
+#else
+  CrossplatformThread(void (*ptr)(void *),
+                      void *params,
+                      const char *const name = "OkapiLibCrossplatformTask")
+#endif
+    :
+#ifdef THREADS_STD
+      thread(ptr, params)
+#else
+      thread(
+        pros::c::task_create(ptr, params, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, name))
+#endif
+  {
+  }
+
+  ~CrossplatformThread() {
+#ifdef THREADS_STD
+    thread.join();
+#else
+    if (pros::c::task_get_state(thread) != pros::E_TASK_STATE_DELETED) {
+      pros::c::task_delete(thread);
+    }
+#endif
+  }
+
+#ifdef THREADS_STD
+  void notifyWhenDeleting(CrossplatformThread *) {
+  }
+#else
+  void notifyWhenDeleting(CrossplatformThread *parent) {
+    pros::c::task_notify_when_deleting(parent->thread, thread, 1, pros::E_NOTIFY_ACTION_INCR);
+  }
+#endif
+
+#ifdef THREADS_STD
+  void notifyWhenDeletingRaw(CROSSPLATFORM_THREAD_T *) {
+  }
+#else
+  void notifyWhenDeletingRaw(CROSSPLATFORM_THREAD_T parent) {
+    pros::c::task_notify_when_deleting(parent, thread, 1, pros::E_NOTIFY_ACTION_INCR);
+  }
+#endif
+
+#ifdef THREADS_STD
+  std::uint32_t notifyTake(const std::uint32_t) {
+    return 0;
+  }
+#else
+  std::uint32_t notifyTake(const std::uint32_t itimeout) {
+    return pros::c::task_notify_take(true, itimeout);
+  }
+#endif
+
+  static std::string getName() {
+#ifdef THREADS_STD
+    std::ostringstream ss;
+    ss << std::this_thread::get_id();
+    return ss.str();
+#else
+    return std::string(pros::c::task_get_name(NULL));
+#endif
+  }
+
+  CROSSPLATFORM_THREAD_T thread;
+};
+
+class CrossplatformMutex {
+  public:
+  CrossplatformMutex() = default;
+
+  void lock() {
+#ifdef THREADS_STD
+    mutex.lock();
+#else
+    while (!mutex.take(1)) {
+    }
+#endif
+  }
+
+  void unlock() {
+#ifdef THREADS_STD
+    mutex.unlock();
+#else
+    mutex.give();
+#endif
+  }
+
+  protected:
+  CROSSPLATFORM_MUTEX_T mutex;
+};

SerialTest/include/okapi/api/device/button/abstractButton.hpp

@@ -0,0 +1,46 @@
+/*
+ * 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 "okapi/api/control/controllerInput.hpp"
+
+namespace okapi {
+class AbstractButton : public ControllerInput<bool> {
+  public:
+  virtual ~AbstractButton();
+
+  /**
+   * Return whether the button is currently pressed.
+   **/
+  virtual bool isPressed() = 0;
+
+  /**
+   * Return whether the state of the button changed since the last time this method was
+   * called.
+   **/
+  virtual bool changed() = 0;
+
+  /**
+   * Return whether the state of the button changed to being pressed since the last time this method
+   * was called.
+   **/
+  virtual bool changedToPressed() = 0;
+
+  /**
+   * Return whether the state of the button to being not pressed changed since the last time this
+   * method was called.
+   **/
+  virtual bool changedToReleased() = 0;
+
+  /**
+   * Get the sensor value for use in a control loop. This method might be automatically called in
+   * another thread by the controller.
+   *
+   * @return the current sensor value. This is the same as the output of the pressed() method.
+   */
+  virtual bool controllerGet() override;
+};
+} // namespace okapi

SerialTest/include/okapi/api/device/button/buttonBase.hpp

@@ -0,0 +1,52 @@
+/*
+ * 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 "okapi/api/device/button/abstractButton.hpp"
+
+namespace okapi {
+class ButtonBase : public AbstractButton {
+  public:
+  /**
+   * @param iinverted Whether the button is inverted (`true` meaning default pressed and `false`
+   * meaning default not pressed).
+   */
+  explicit ButtonBase(bool iinverted = false);
+
+  /**
+   * Return whether the button is currently pressed.
+   **/
+  bool isPressed() override;
+
+  /**
+   * Return whether the state of the button changed since the last time this method was called.
+   **/
+  bool changed() override;
+
+  /**
+   * Return whether the state of the button changed to pressed since the last time this method was
+   *called.
+   **/
+  bool changedToPressed() override;
+
+  /**
+   * Return whether the state of the button to not pressed since the last time this method was
+   *called.
+   **/
+  bool changedToReleased() override;
+
+  protected:
+  bool inverted{false};
+  bool wasPressedLast_c{false};
+  bool wasPressedLast_ctp{false};
+  bool wasPressedLast_ctr{false};
+
+  virtual bool currentlyPressed() = 0;
+
+  private:
+  bool changedImpl(bool &prevState);
+};
+} // namespace okapi

SerialTest/include/okapi/api/device/motor/abstractMotor.hpp

@@ -0,0 +1,537 @@
+/*
+ * 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 "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/device/rotarysensor/continuousRotarySensor.hpp"
+#include <memory>
+
+namespace okapi {
+class AbstractMotor : public ControllerOutput<double> {
+  public:
+  /**
+   * Indicates the 'brake mode' of a motor.
+   */
+  enum class brakeMode {
+    coast = 0, ///< Motor coasts when stopped, traditional behavior
+    brake = 1, ///< Motor brakes when stopped
+    hold = 2,  ///< Motor actively holds position when stopped
+    invalid = INT32_MAX
+  };
+
+  /**
+   * Indicates the units used by the motor encoders.
+   */
+  enum class encoderUnits {
+    degrees = 0,        ///< degrees
+    rotations = 1,      ///< rotations
+    counts = 2,         ///< counts
+    invalid = INT32_MAX ///< invalid
+  };
+
+  /**
+   * Indicates the internal gear ratio of a motor.
+   */
+  enum class gearset {
+    red = 100,   ///< 36:1, 100 RPM, Red gear set
+    green = 200, ///< 18:1, 200 RPM, Green gear set
+    blue = 600,  ///< 6:1,  600 RPM, Blue gear set
+    invalid = INT32_MAX
+  };
+
+  /**
+   * A simple structure representing the full ratio between motor and wheel.
+   */
+  struct GearsetRatioPair {
+    /**
+     * A simple structure representing the full ratio between motor and wheel.
+     *
+     * The ratio is `motor rotation : wheel rotation`, e.x., if one motor rotation
+     * corresponds to two wheel rotations, the ratio is `1.0/2.0`.
+     *
+     * @param igearset The gearset in the motor.
+     * @param iratio The ratio of motor rotation to wheel rotation.
+     */
+    GearsetRatioPair(const gearset igearset, const double iratio = 1)
+      : internalGearset(igearset), ratio(iratio) {
+    }
+
+    ~GearsetRatioPair() = default;
+
+    gearset internalGearset;
+    double ratio = 1;
+  };
+
+  virtual ~AbstractMotor();
+
+  /******************************************************************************/
+  /**                         Motor movement functions                         **/
+  /**                                                                          **/
+  /**          These functions allow programmers to make motors move           **/
+  /******************************************************************************/
+
+  /**
+   * Sets the target absolute position for the motor to move to.
+   *
+   * This movement is relative to the position of the motor when initialized or
+   * the position when it was most recently reset with setZeroPosition().
+   *
+   * @note This function simply sets the target for the motor, it does not block program execution
+   * until the movement finishes.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param iposition The absolute position to move to in the motor's encoder units
+   * @param ivelocity The maximum allowable velocity for the movement in RPM
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t moveAbsolute(double iposition, std::int32_t ivelocity) = 0;
+
+  /**
+   * Sets the relative target position for the motor to move to.
+   *
+   * This movement is relative to the current position of the motor. Providing 10.0 as the position
+   * parameter would result in the motor moving clockwise 10 units, no matter what the current
+   * position is.
+   *
+   * @note This function simply sets the target for the motor, it does not block program execution
+   * until the movement finishes.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param iposition The relative position to move to in the motor's encoder units
+   * @param ivelocity The maximum allowable velocity for the movement in RPM
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t moveRelative(double iposition, std::int32_t ivelocity) = 0;
+
+  /**
+   * Sets the velocity for the motor.
+   *
+   * This velocity corresponds to different actual speeds depending on the gearset
+   * used for the motor. This results in a range of +-100 for pros::c::red,
+   * +-200 for green, and +-600 for blue. The velocity
+   * is held with PID to ensure consistent speed, as opposed to setting the motor's
+   * voltage.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ivelocity The new motor velocity from -+-100, +-200, or +-600 depending on the motor's
+   * gearset
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t moveVelocity(std::int16_t ivelocity) = 0;
+
+  /**
+   * Sets the voltage for the motor from -12000 to 12000.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ivoltage The new voltage value from -12000 to 12000
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t moveVoltage(std::int16_t ivoltage) = 0;
+
+  /**
+   * Changes the output velocity for a profiled movement (moveAbsolute or moveRelative). This will
+   * have no effect if the motor is not following a profiled movement.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ivelocity The new motor velocity from -+-100, +-200, or +-600 depending on the motor's
+   * gearset
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t modifyProfiledVelocity(std::int32_t ivelocity) = 0;
+
+  /******************************************************************************/
+  /**                        Motor telemetry functions                         **/
+  /**                                                                          **/
+  /**    These functions allow programmers to collect telemetry from motors    **/
+  /******************************************************************************/
+
+  /**
+   * Gets the target position set for the motor by the user.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The target position in its encoder units or PROS_ERR_F if the operation failed,
+   * setting errno.
+   */
+  virtual double getTargetPosition() = 0;
+
+  /**
+   * Gets the absolute position of the motor in its encoder units.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's absolute position in its encoder units or PROS_ERR_F if the operation
+   * failed, setting errno.
+   */
+  virtual double getPosition() = 0;
+
+  /**
+   * Gets the positional error (target position minus actual position) of the motor in its encoder
+   * units.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's positional error in its encoder units or PROS_ERR_F if the operation
+   * failed, setting errno.
+   */
+  double getPositionError();
+
+  /**
+   * Sets the "absolute" zero position of the motor to its current position.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t tarePosition() = 0;
+
+  /**
+   * Gets the velocity commanded to the motor by the user.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The commanded motor velocity from +-100, +-200, or +-600, or PROS_ERR if the operation
+   * failed, setting errno.
+   */
+  virtual std::int32_t getTargetVelocity() = 0;
+
+  /**
+   * Gets the actual velocity of the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's actual velocity in RPM or PROS_ERR_F if the operation failed, setting
+   * errno.
+   */
+  virtual double getActualVelocity() = 0;
+
+  /**
+   * Gets the difference between the target velocity of the motor and the actual velocity of the
+   * motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's velocity error in RPM or PROS_ERR_F if the operation failed, setting
+   * errno.
+   */
+  double getVelocityError();
+
+  /**
+   * Gets the current drawn by the motor in mA.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's current in mA or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t getCurrentDraw() = 0;
+
+  /**
+   * Gets the direction of movement for the motor.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return 1 for moving in the positive direction, -1 for moving in the negative direction, and
+   * PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t getDirection() = 0;
+
+  /**
+   * Gets the efficiency of the motor in percent.
+   *
+   * An efficiency of 100% means that the motor is moving electrically while
+   * drawing no electrical power, and an efficiency of 0% means that the motor
+   * is drawing power but not moving.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's efficiency in percent or PROS_ERR_F if the operation failed, setting errno.
+   */
+  virtual double getEfficiency() = 0;
+
+  /**
+   * Checks if the motor is drawing over its current limit.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return 1 if the motor's current limit is being exceeded and 0 if the current limit is not
+   * exceeded, or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t isOverCurrent() = 0;
+
+  /**
+   * Checks if the motor's temperature is above its limit.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return 1 if the temperature limit is exceeded and 0 if the the temperature is below the limit,
+   * or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t isOverTemp() = 0;
+
+  /**
+   * Checks if the motor is stopped.
+   *
+   * Although this function forwards data from the motor, the motor presently does not provide any
+   * value. This function returns PROS_ERR with errno set to ENOSYS.
+   *
+   * @return 1 if the motor is not moving, 0 if the motor is moving, or PROS_ERR if the operation
+   * failed, setting errno
+   */
+  virtual std::int32_t isStopped() = 0;
+
+  /**
+   * Checks if the motor is at its zero position.
+   *
+   * Although this function forwards data from the motor, the motor presently does not provide any
+   * value. This function returns PROS_ERR with errno set to ENOSYS.
+   *
+   * @return 1 if the motor is at zero absolute position, 0 if the motor has moved from its absolute
+   * zero, or PROS_ERR if the operation failed, setting errno
+   */
+  virtual std::int32_t getZeroPositionFlag() = 0;
+
+  /**
+   * Gets the faults experienced by the motor. Compare this bitfield to the bitmasks in
+   * pros::motor_fault_e_t.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return A currently unknown bitfield containing the motor's faults. 0b00000100 = Current Limit
+   * Hit
+   */
+  virtual uint32_t getFaults() = 0;
+
+  /**
+   * Gets the flags set by the motor's operation. Compare this bitfield to the bitmasks in
+   * pros::motor_flag_e_t.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return A currently unknown bitfield containing the motor's flags. These seem to be unrelated
+   * to the individual get_specific_flag functions
+   */
+  virtual uint32_t getFlags() = 0;
+
+  /**
+   * Gets the raw encoder count of the motor at a given timestamp.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param timestamp A pointer to a time in milliseconds for which the encoder count will be
+   * returned. If NULL, the timestamp at which the encoder count was read will not be supplied
+   *
+   * @return The raw encoder count at the given timestamp or PROS_ERR if the operation failed.
+   */
+  virtual std::int32_t getRawPosition(std::uint32_t *timestamp) = 0;
+
+  /**
+   * Gets the power drawn by the motor in Watts.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's power draw in Watts or PROS_ERR_F if the operation failed, setting errno.
+   */
+  virtual double getPower() = 0;
+
+  /**
+   * Gets the temperature of the motor in degrees Celsius.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's temperature in degrees Celsius or PROS_ERR_F if the operation failed,
+   * setting errno.
+   */
+  virtual double getTemperature() = 0;
+
+  /**
+   * Gets the torque generated by the motor in Newton Metres (Nm).
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's torque in NM or PROS_ERR_F if the operation failed, setting errno.
+   */
+  virtual double getTorque() = 0;
+
+  /**
+   * Gets the voltage delivered to the motor in millivolts.
+   *
+   * This function uses the following values of errno when an error state is
+   * reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's voltage in V or PROS_ERR_F if the operation failed, setting errno.
+   */
+  virtual std::int32_t getVoltage() = 0;
+
+  /******************************************************************************/
+  /**                      Motor configuration functions                       **/
+  /**                                                                          **/
+  /**  These functions allow programmers to configure the behavior of motors   **/
+  /******************************************************************************/
+
+  /**
+   * Sets one of brakeMode to the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param imode The new motor brake mode to set for the motor
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setBrakeMode(brakeMode imode) = 0;
+
+  /**
+   * Gets the brake mode that was set for the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return One of brakeMode, according to what was set for the motor, or brakeMode::invalid if the
+   * operation failed, setting errno.
+   */
+  virtual brakeMode getBrakeMode() = 0;
+
+  /**
+   * Sets the current limit for the motor in mA.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ilimit The new current limit in mA
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setCurrentLimit(std::int32_t ilimit) = 0;
+
+  /**
+   * Gets the current limit for the motor in mA.
+   *
+   * The default value is 2500 mA.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return The motor's current limit in mA or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t getCurrentLimit() = 0;
+
+  /**
+   * Sets one of encoderUnits for the motor encoder.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param iunits The new motor encoder units
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setEncoderUnits(encoderUnits iunits) = 0;
+
+  /**
+   * Gets the encoder units that were set for the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return One of encoderUnits according to what is set for the motor or encoderUnits::invalid if
+   * the operation failed.
+   */
+  virtual encoderUnits getEncoderUnits() = 0;
+
+  /**
+   * Sets one of gearset for the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param igearset The new motor gearset
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setGearing(gearset igearset) = 0;
+
+  /**
+   * Gets the gearset that was set for the motor.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @return One of gearset according to what is set for the motor, or gearset::invalid if the
+   * operation failed.
+   */
+  virtual gearset getGearing() = 0;
+
+  /**
+   * Sets the reverse flag for the motor.
+   *
+   * This will invert its movements and the values returned for its position.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ireverse True reverses the motor, false is default
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setReversed(bool ireverse) = 0;
+
+  /**
+   * Sets the voltage limit for the motor in Volts.
+   *
+   * This function uses the following values of errno when an error state is reached:
+   * EACCES - Another resource is currently trying to access the port.
+   *
+   * @param ilimit The new voltage limit in Volts
+   * @return 1 if the operation was successful or PROS_ERR if the operation failed, setting errno.
+   */
+  virtual std::int32_t setVoltageLimit(std::int32_t ilimit) = 0;
+
+  /**
+   * Returns the encoder associated with this motor.
+   *
+   * @return the encoder for this motor
+   */
+  virtual std::shared_ptr<ContinuousRotarySensor> getEncoder() = 0;
+};
+
+AbstractMotor::GearsetRatioPair operator*(AbstractMotor::gearset gearset, double ratio);
+
+} // namespace okapi

SerialTest/include/okapi/api/device/rotarysensor/continuousRotarySensor.hpp

@@ -0,0 +1,20 @@
+/*
+ * 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 "okapi/api/device/rotarysensor/rotarySensor.hpp"
+
+namespace okapi {
+class ContinuousRotarySensor : public RotarySensor {
+  public:
+  /**
+   * Reset the sensor to zero.
+   *
+   * @return `1` on success, `PROS_ERR` on fail
+   */
+  virtual std::int32_t reset() = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/device/rotarysensor/rotarySensor.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 "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/coreProsAPI.hpp"
+
+namespace okapi {
+class RotarySensor : public ControllerInput<double> {
+  public:
+  virtual ~RotarySensor();
+
+  /**
+   * Get the current sensor value.
+   *
+   * @return the current sensor value, or `PROS_ERR` on a failure.
+   */
+  virtual double get() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/averageFilter.hpp

@@ -0,0 +1,59 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+#include <array>
+#include <cstddef>
+
+namespace okapi {
+/**
+ * A filter which returns the average of a list of values.
+ *
+ * @tparam n number of taps in the filter
+ */
+template <std::size_t n> class AverageFilter : public Filter {
+  public:
+  /**
+   * Averaging filter.
+   */
+  AverageFilter() = default;
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param ireading new measurement
+   * @return filtered result
+   */
+  double filter(const double ireading) override {
+    data[index++] = ireading;
+    if (index >= n) {
+      index = 0;
+    }
+
+    output = 0.0;
+    for (size_t i = 0; i < n; i++)
+      output += data[i];
+    output /= (double)n;
+
+    return output;
+  }
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override {
+    return output;
+  }
+
+  protected:
+  std::array<double, n> data{0};
+  std::size_t index = 0;
+  double output = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/composableFilter.hpp

@@ -0,0 +1,50 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+#include <functional>
+#include <initializer_list>
+#include <memory>
+#include <vector>
+
+namespace okapi {
+class ComposableFilter : public Filter {
+  public:
+  /**
+   * A composable filter is a filter that consists of other filters. The input signal is passed
+   * through each filter in sequence. The final output of this filter is the output of the last
+   * filter.
+   *
+   * @param ilist The filters to use in sequence.
+   */
+  ComposableFilter(const std::initializer_list<std::shared_ptr<Filter>> &ilist);
+
+  /**
+   * Filters a value.
+   *
+   * @param ireading A new measurement.
+   * @return The filtered result.
+   */
+  double filter(double ireading) override;
+
+  /**
+   * @return The previous output from filter.
+   */
+  double getOutput() const override;
+
+  /**
+   * Adds a filter to the end of the sequence.
+   *
+   * @param ifilter The filter to add.
+   */
+  virtual void addFilter(std::shared_ptr<Filter> ifilter);
+
+  protected:
+  std::vector<std::shared_ptr<Filter>> filters;
+  double output = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/demaFilter.hpp

@@ -0,0 +1,52 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+#include <ratio>
+
+namespace okapi {
+class DemaFilter : public Filter {
+  public:
+  /**
+   * Double exponential moving average filter.
+   *
+   * @param ialpha alpha gain
+   * @param ibeta beta gain
+   */
+  DemaFilter(double ialpha, double ibeta);
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param reading new measurement
+   * @return filtered result
+   */
+  double filter(double ireading) override;
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override;
+
+  /**
+   * Set filter gains.
+   *
+   * @param ialpha alpha gain
+   * @param ibeta beta gain
+   */
+  virtual void setGains(double ialpha, double ibeta);
+
+  protected:
+  double alpha, beta;
+  double outputS = 0;
+  double lastOutputS = 0;
+  double outputB = 0;
+  double lastOutputB = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/ekfFilter.hpp

@@ -0,0 +1,71 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+#include "okapi/api/util/mathUtil.hpp"
+
+namespace okapi {
+class EKFFilter : public Filter {
+  public:
+  /**
+   * One dimensional extended Kalman filter. The default arguments should work fine for most signal
+   * filtering. It won't hurt to graph your signal and the filtered result, and check if the filter
+   * is doing its job.
+   *
+   * Q is the covariance of the process noise and R is the
+   * covariance of the observation noise. The default values for Q and R should be a modest balance
+   * between trust in the sensor and FIR filtering.
+   *
+   * Think of R as how noisy your sensor is. Its value can be found mathematically by computing the
+   * standard deviation of your sensor reading vs. "truth" (of course, "truth" is still an estimate;
+   * try to calibrate your robot in a controlled setting where you can minimize the error in what
+   * "truth" is).
+   *
+   * Think of Q as how noisy your model is. It decides how much "smoothing" the filter does and how
+   * far it lags behind the true signal. This parameter is most often used as a "tuning" parameter
+   * to adjust the response of the filter.
+   *
+   * @param iQ process noise covariance
+   * @param iR measurement noise covariance
+   */
+  explicit EKFFilter(double iQ = 0.0001, double iR = ipow(0.2, 2));
+
+  /**
+   * Filters a value, like a sensor reading. Assumes the control input is zero.
+   *
+   * @param ireading new measurement
+   * @return filtered result
+   */
+  double filter(double ireading) override;
+
+  /**
+   * Filters a reading with a control input.
+   *
+   * @param ireading new measurement
+   * @param icontrol control input
+   * @return filtered result
+   */
+  virtual double filter(double ireading, double icontrol);
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override;
+
+  protected:
+  const double Q, R;
+  double xHat = 0;
+  double xHatPrev = 0;
+  double xHatMinus = 0;
+  double P = 0;
+  double Pprev = 1;
+  double Pminus = 0;
+  double K = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/emaFilter.hpp

@@ -0,0 +1,47 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+
+namespace okapi {
+class EmaFilter : public Filter {
+  public:
+  /**
+   * Exponential moving average filter.
+   *
+   * @param ialpha alpha gain
+   */
+  explicit EmaFilter(double ialpha);
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param reading new measurement
+   * @return filtered result
+   */
+  double filter(double ireading) override;
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override;
+
+  /**
+   * Set filter gains.
+   *
+   * @param ialpha alpha gain
+   */
+  virtual void setGains(double ialpha);
+
+  protected:
+  double alpha;
+  double output = 0;
+  double lastOutput = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/filter.hpp

@@ -0,0 +1,28 @@
+/*
+ * 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
+
+namespace okapi {
+class Filter {
+  public:
+  virtual ~Filter();
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param ireading new measurement
+   * @return filtered result
+   */
+  virtual double filter(double ireading) = 0;
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  virtual double getOutput() const = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/filteredControllerInput.hpp

@@ -0,0 +1,48 @@
+/*
+ * 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 "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/filter/filter.hpp"
+#include <memory>
+
+namespace okapi {
+/**
+ * A ControllerInput with a filter built in.
+ *
+ * @tparam InputType the type of the ControllerInput
+ * @tparam FilterType the type of the Filter
+ */
+template <typename InputType, typename FilterType>
+class FilteredControllerInput : public ControllerInput<double> {
+  public:
+  /**
+   * A filtered controller input. Applies a filter to the controller input. Useful if you want to
+   * place a filter between a control input and a control loop.
+   *
+   * @param iinput ControllerInput type
+   * @param ifilter Filter type
+   */
+  FilteredControllerInput(std::unique_ptr<ControllerInput<InputType>> iinput,
+                          std::unique_ptr<FilterType> ifilter)
+    : input(std::move(iinput)), filter(std::move(ifilter)) {
+  }
+
+  /**
+   * Gets the sensor value for use in a control loop. This method might be automatically called in
+   * another thread by the controller.
+   *
+   * @return the current filtered sensor value.
+   */
+  double controllerGet() override {
+    return filter->filter(input->controllerGet());
+  }
+
+  protected:
+  std::unique_ptr<ControllerInput<InputType>> input;
+  std::unique_ptr<FilterType> filter;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/medianFilter.hpp

@@ -0,0 +1,94 @@
+/*
+ * Uses the median filter algorithm from N. Wirth’s book, implementation by N. Devillard.
+ *
+ * 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 "okapi/api/filter/filter.hpp"
+#include <algorithm>
+#include <array>
+#include <cstddef>
+
+namespace okapi {
+/**
+ * A filter which returns the median value of list of values.
+ *
+ * @tparam n number of taps in the filter
+ */
+template <std::size_t n> class MedianFilter : public Filter {
+  public:
+  MedianFilter() : middleIndex((((n)&1) ? ((n) / 2) : (((n) / 2) - 1))) {
+  }
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param ireading new measurement
+   * @return filtered result
+   */
+  double filter(const double ireading) override {
+    data[index++] = ireading;
+    if (index >= n) {
+      index = 0;
+    }
+
+    output = kth_smallset();
+    return output;
+  }
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override {
+    return output;
+  }
+
+  protected:
+  std::array<double, n> data{0};
+  std::size_t index = 0;
+  double output = 0;
+  const size_t middleIndex;
+
+  /**
+   * Algorithm from N. Wirth’s book, implementation by N. Devillard.
+   */
+  double kth_smallset() {
+    std::array<double, n> dataCopy = data;
+    size_t j, l, m;
+    l = 0;
+    m = n - 1;
+
+    while (l < m) {
+      double x = dataCopy[middleIndex];
+      size_t i = l;
+      j = m;
+      do {
+        while (dataCopy[i] < x) {
+          i++;
+        }
+        while (x < dataCopy[j]) {
+          j--;
+        }
+        if (i <= j) {
+          const double t = dataCopy[i];
+          dataCopy[i] = dataCopy[j];
+          dataCopy[j] = t;
+          i++;
+          j--;
+        }
+      } while (i <= j);
+      if (j < middleIndex)
+        l = i;
+      if (middleIndex < i)
+        m = j;
+    }
+
+    return dataCopy[middleIndex];
+  }
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/passthroughFilter.hpp

@@ -0,0 +1,36 @@
+/*
+ * 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 "okapi/api/filter/filter.hpp"
+
+namespace okapi {
+class PassthroughFilter : public Filter {
+  public:
+  /**
+   * A simple filter that does no filtering and just passes the input through.
+   */
+  PassthroughFilter();
+
+  /**
+   * Filters a value, like a sensor reading.
+   *
+   * @param ireading new measurement
+   * @return filtered result
+   */
+  double filter(double ireading) override;
+
+  /**
+   * Returns the previous output from filter.
+   *
+   * @return the previous output from filter
+   */
+  double getOutput() const override;
+
+  protected:
+  double lastOutput = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/filter/velMath.hpp

@@ -0,0 +1,74 @@
+/*
+ * 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 "okapi/api/filter/composableFilter.hpp"
+#include "okapi/api/units/QAngularAcceleration.hpp"
+#include "okapi/api/units/QAngularSpeed.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include "okapi/api/util/logging.hpp"
+#include <memory>
+
+namespace okapi {
+class VelMath {
+  public:
+  /**
+   * Velocity math helper. Calculates filtered velocity. Throws a `std::invalid_argument` exception
+   * if `iticksPerRev` is zero.
+   *
+   * @param iticksPerRev The number of ticks per revolution (or whatever units you are using).
+   * @param ifilter The filter used for filtering the calculated velocity.
+   * @param isampleTime The minimum time between velocity measurements.
+   * @param ilogger The logger this instance will log to.
+   */
+  VelMath(double iticksPerRev,
+          std::unique_ptr<Filter> ifilter,
+          QTime isampleTime,
+          std::unique_ptr<AbstractTimer> iloopDtTimer,
+          std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  virtual ~VelMath();
+
+  /**
+   * Calculates the current velocity and acceleration. Returns the (filtered) velocity.
+   *
+   * @param inewPos The new position measurement.
+   * @return The new velocity estimate.
+   */
+  virtual QAngularSpeed step(double inewPos);
+
+  /**
+   * Sets ticks per revolution (or whatever units you are using). Throws a `std::invalid_argument`
+   * exception if iticksPerRev is zero.
+   *
+   * @param iTPR The number of ticks per revolution.
+   */
+  virtual void setTicksPerRev(double iTPR);
+
+  /**
+   * @return The last calculated velocity.
+   */
+  virtual QAngularSpeed getVelocity() const;
+
+  /**
+   * @return The last calculated acceleration.
+   */
+  virtual QAngularAcceleration getAccel() const;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  QAngularSpeed vel{0_rpm};
+  QAngularSpeed lastVel{0_rpm};
+  QAngularAcceleration accel{0.0};
+  double lastPos{0};
+  double ticksPerRev;
+
+  QTime sampleTime;
+  std::unique_ptr<AbstractTimer> loopDtTimer;
+  std::unique_ptr<Filter> filter;
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/odomMath.hpp

@@ -0,0 +1,95 @@
+/*
+ * 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 "okapi/api/odometry/odomState.hpp"
+#include "okapi/api/odometry/point.hpp"
+#include "okapi/api/util/logging.hpp"
+#include <tuple>
+
+namespace okapi {
+class OdomMath {
+  public:
+  /**
+   * Computes the distance from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance between the Odometry state and the point.
+   */
+  static QLength computeDistanceToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the angle from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The angle between the Odometry state and the point.
+   */
+  static QAngle computeAngleToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance and angle from the given Odometry state to the given point. The point and
+   * the OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance and angle between the Odometry state and the point.
+   */
+  static std::pair<QLength, QAngle> computeDistanceAndAngleToPoint(const Point &ipoint,
+                                                                   const OdomState &istate);
+
+  /**
+   * Constraints the angle to [0,360] degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [0,360] degrees.
+   */
+  static QAngle constrainAngle360(const QAngle &angle);
+
+  /**
+   * Constraints the angle to [-180,180) degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [-180,180) degrees.
+   */
+  static QAngle constrainAngle180(const QAngle &angle);
+
+  private:
+  OdomMath();
+  ~OdomMath();
+
+  /**
+   * Computes the x and y diffs in meters between the points.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The diffs in the order `{xDiff, yDiff}`.
+   */
+  static std::pair<double, double> computeDiffs(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @return The cartesian distance in meters.
+   */
+  static double computeDistance(double xDiff, double yDiff);
+
+  /**
+   * Compites the angle between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @param theta The current robot's theta in radians.
+   * @return The angle in radians.
+   */
+  static double computeAngle(double xDiff, double yDiff, double theta);
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/odomState.hpp

@@ -0,0 +1,57 @@
+/*
+ * 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 "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/RQuantityName.hpp"
+#include <string>
+
+namespace okapi {
+struct OdomState {
+  QLength x{0_m};
+  QLength y{0_m};
+  QAngle theta{0_deg};
+
+  /**
+   * Get a string for the current odometry state (optionally with the specified units).
+   *
+   * Examples:
+   *   - `OdomState::str(1_m, 1_deg)`: The default (no arguments specified).
+   *   - `OdomState::str(1_tile, 1_radian)`: distance tiles and angle radians.
+   *
+   * Throws std::domain_error if the units passed are undefined.
+   *
+   * @param idistanceUnit The units you want your distance to be in. This must be an exact, predefined QLength (such as foot, meter, inch, tile etc.).
+   * @param iangleUnit The units you want your angle to be in. This must be an exact, predefined QAngle (degree or radian).
+   * @return A string representing the state.
+   */
+  std::string str(QLength idistanceUnit, QAngle iangleUnit) const;
+
+  /**
+   * Get a string for the current odometry state (optionally with the specified units).
+   *
+   * Examples:
+   *   - `OdomState::str(1_m, "_m", 1_deg, "_deg")`: The default (no arguments specified), prints in meters and degrees.
+   *   - `OdomState::str(1_in, "_in", 1_deg, "_deg")` or `OdomState::str(1_in, "\"", 1_deg, "°")`: to print values in inches and degrees with different suffixes.
+   *   - `OdomState::str(6_tile / 100, "%", 360_deg / 100, "%")` to get the distance values in % of the vex field, and angle values in % of a full rotation.
+   *
+   * @param idistanceUnit The units you want your distance to be in. The x or y position will be output in multiples of this length.
+   * @param distUnitName The suffix you as your distance unit.
+   * @param iangleUnit The units you want your angle to be in. The angle will be output in multiples of this unit.
+   * @param angleUnitName The suffix you want as your angle unit.
+   * @return A string representing the state.
+   */
+  std::string str(QLength idistanceUnit = meter,
+                  std::string distUnitName = "_m",
+                  QAngle iangleUnit = degree,
+                  std::string angleUnitName = "_deg") const;
+
+  bool operator==(const OdomState &rhs) const;
+
+  bool operator!=(const OdomState &rhs) const;
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/odometry.hpp

@@ -0,0 +1,61 @@
+/*
+ * 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 "okapi/api/chassis/controller/chassisScales.hpp"
+#include "okapi/api/chassis/model/readOnlyChassisModel.hpp"
+#include "okapi/api/odometry/odomState.hpp"
+#include "okapi/api/odometry/stateMode.hpp"
+
+namespace okapi {
+class Odometry {
+  public:
+  /**
+   * Odometry. Tracks the movement of the robot and estimates its position in coordinates
+   * relative to the start (assumed to be (0, 0, 0)).
+   */
+  explicit Odometry() = default;
+
+  virtual ~Odometry() = default;
+
+  /**
+   * Sets the drive and turn scales.
+   */
+  virtual void setScales(const ChassisScales &ichassisScales) = 0;
+
+  /**
+   * Do one odometry step.
+   */
+  virtual void step() = 0;
+
+  /**
+   * Returns the current state.
+   *
+   * @param imode The mode to return the state in.
+   * @return The current state in the given format.
+   */
+  virtual OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const = 0;
+
+  /**
+   * Sets a new state to be the current state.
+   *
+   * @param istate The new state in the given format.
+   * @param imode The mode to treat the input state as.
+   */
+  virtual void setState(const OdomState &istate,
+                        const StateMode &imode = StateMode::FRAME_TRANSFORMATION) = 0;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  virtual std::shared_ptr<ReadOnlyChassisModel> getModel() = 0;
+
+  /**
+   * @return The internal ChassisScales.
+   */
+  virtual ChassisScales getScales() = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/point.hpp

@@ -0,0 +1,30 @@
+/*
+ * 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 "okapi/api/odometry/stateMode.hpp"
+#include "okapi/api/units/QLength.hpp"
+
+namespace okapi {
+struct Point {
+  QLength x{0_m};
+  QLength y{0_m};
+
+  /**
+   * Computes the value of this point in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param imode The StateMode this Point is currently specified in.
+   * @return This point specified in `StateMode::FRAME_TRANSFORMATION`.
+   */
+  Point inFT(const StateMode &imode) const {
+    if (imode == StateMode::FRAME_TRANSFORMATION) {
+      return *this;
+    } else {
+      return {y, x};
+    }
+  }
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/stateMode.hpp

@@ -0,0 +1,17 @@
+/*
+ * 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
+
+namespace okapi {
+/**
+ * The mode for the OdomState calculated by Odometry.
+ */
+enum class StateMode {
+  FRAME_TRANSFORMATION, ///< +x is forward, +y is right, 0 degrees is along +x
+  CARTESIAN             ///< +x is right, +y is forward, 0 degrees is along +y
+};
+
+} // namespace okapi

SerialTest/include/okapi/api/odometry/threeEncoderOdometry.hpp

@@ -0,0 +1,43 @@
+/*
+ * 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 "okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp"
+#include "okapi/api/odometry/twoEncoderOdometry.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <functional>
+
+namespace okapi {
+class ThreeEncoderOdometry : public TwoEncoderOdometry {
+  public:
+  /**
+   * Odometry. Tracks the movement of the robot and estimates its position in coordinates
+   * relative to the start (assumed to be (0, 0)).
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The chassis model for reading sensors.
+   * @param ichassisScales See ChassisScales docs (the middle wheel scale is the third member)
+   * @param iwheelVelDelta The maximum delta between wheel velocities to consider the robot as
+   * driving straight.
+   * @param ilogger The logger this instance will log to.
+   */
+  ThreeEncoderOdometry(const TimeUtil &itimeUtil,
+                       const std::shared_ptr<ReadOnlyChassisModel> &imodel,
+                       const ChassisScales &ichassisScales,
+                       const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  protected:
+  /**
+   * Does the math, side-effect free, for one odom step.
+   *
+   * @param itickDiff The tick difference from the previous step to this step.
+   * @param ideltaT The time difference from the previous step to this step.
+   * @return The newly computed OdomState.
+   */
+  OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
+                         const QTime &ideltaT) override;
+};
+} // namespace okapi

SerialTest/include/okapi/api/odometry/twoEncoderOdometry.hpp

@@ -0,0 +1,93 @@
+/*
+ * 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 "okapi/api/odometry/odometry.hpp"
+#include "okapi/api/units/QSpeed.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <atomic>
+#include <memory>
+#include <valarray>
+
+namespace okapi {
+class TwoEncoderOdometry : public Odometry {
+  public:
+  /**
+   * TwoEncoderOdometry. Tracks the movement of the robot and estimates its position in coordinates
+   * relative to the start (assumed to be (0, 0, 0)).
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The chassis model for reading sensors.
+   * @param ichassisScales The chassis dimensions.
+   * @param ilogger The logger this instance will log to.
+   */
+  TwoEncoderOdometry(const TimeUtil &itimeUtil,
+                     const std::shared_ptr<ReadOnlyChassisModel> &imodel,
+                     const ChassisScales &ichassisScales,
+                     const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  virtual ~TwoEncoderOdometry() = default;
+
+  /**
+   * Sets the drive and turn scales.
+   */
+  void setScales(const ChassisScales &ichassisScales) override;
+
+  /**
+   * Do one odometry step.
+   */
+  void step() override;
+
+  /**
+   * Returns the current state.
+   *
+   * @param imode The mode to return the state in.
+   * @return The current state in the given format.
+   */
+  OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const override;
+
+  /**
+   * Sets a new state to be the current state.
+   *
+   * @param istate The new state in the given format.
+   * @param imode The mode to treat the input state as.
+   */
+  void setState(const OdomState &istate,
+                const StateMode &imode = StateMode::FRAME_TRANSFORMATION) override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  std::shared_ptr<ReadOnlyChassisModel> getModel() override;
+
+  /**
+   * @return The internal ChassisScales.
+   */
+  ChassisScales getScales() override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::unique_ptr<AbstractRate> rate;
+  std::unique_ptr<AbstractTimer> timer;
+  std::shared_ptr<ReadOnlyChassisModel> model;
+  ChassisScales chassisScales;
+  OdomState state;
+  std::valarray<std::int32_t> newTicks{0, 0, 0}, tickDiff{0, 0, 0}, lastTicks{0, 0, 0};
+  const std::int32_t maximumTickDiff{1000};
+
+  /**
+   * Does the math, side-effect free, for one odom step.
+   *
+   * @param itickDiff The tick difference from the previous step to this step.
+   * @param ideltaT The time difference from the previous step to this step.
+   * @return The newly computed OdomState.
+   */
+  virtual OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
+                                 const QTime &ideltaT);
+};
+} // namespace okapi

SerialTest/include/okapi/api/units/QAcceleration.hpp

@@ -0,0 +1,36 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 1, -2, 0, QAcceleration)
+
+constexpr QAcceleration mps2 = meter / (second * second);
+constexpr QAcceleration G = 9.80665 * mps2;
+
+inline namespace literals {
+constexpr QAcceleration operator"" _mps2(long double x) {
+  return QAcceleration(x);
+}
+constexpr QAcceleration operator"" _mps2(unsigned long long int x) {
+  return QAcceleration(static_cast<double>(x));
+}
+constexpr QAcceleration operator"" _G(long double x) {
+  return static_cast<double>(x) * G;
+}
+constexpr QAcceleration operator"" _G(unsigned long long int x) {
+  return static_cast<double>(x) * G;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QAngle.hpp

@@ -0,0 +1,35 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+#include <cmath>
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, 0, 1, QAngle)
+
+constexpr QAngle radian(1.0);
+constexpr QAngle degree = static_cast<double>(2_pi / 360.0) * radian;
+
+inline namespace literals {
+constexpr QAngle operator"" _rad(long double x) {
+  return QAngle(x);
+}
+constexpr QAngle operator"" _rad(unsigned long long int x) {
+  return QAngle(static_cast<double>(x));
+}
+constexpr QAngle operator"" _deg(long double x) {
+  return static_cast<double>(x) * degree;
+}
+constexpr QAngle operator"" _deg(unsigned long long int x) {
+  return static_cast<double>(x) * degree;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QAngularAcceleration.hpp

@@ -0,0 +1,16 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, -2, 1, QAngularAcceleration)
+}

SerialTest/include/okapi/api/units/QAngularJerk.hpp

@@ -0,0 +1,16 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, -3, 1, QAngularJerk)
+}

SerialTest/include/okapi/api/units/QAngularSpeed.hpp

@@ -0,0 +1,39 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QFrequency.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, -1, 1, QAngularSpeed)
+
+constexpr QAngularSpeed radps = radian / second;
+constexpr QAngularSpeed rpm = (360 * degree) / minute;
+constexpr QAngularSpeed cps = (0.01 * degree) / second; // centidegree per second
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-function"
+static QAngularSpeed convertHertzToRadPerSec(QFrequency in) {
+  return (in.convert(Hz) / 2_pi) * radps;
+}
+#pragma GCC diagnostic pop
+
+inline namespace literals {
+constexpr QAngularSpeed operator"" _rpm(long double x) {
+  return x * rpm;
+}
+constexpr QAngularSpeed operator"" _rpm(unsigned long long int x) {
+  return static_cast<double>(x) * rpm;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QArea.hpp

@@ -0,0 +1,26 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 2, 0, 0, QArea)
+
+constexpr QArea kilometer2 = kilometer * kilometer;
+constexpr QArea meter2 = meter * meter;
+constexpr QArea decimeter2 = decimeter * decimeter;
+constexpr QArea centimeter2 = centimeter * centimeter;
+constexpr QArea millimeter2 = millimeter * millimeter;
+constexpr QArea inch2 = inch * inch;
+constexpr QArea foot2 = foot * foot;
+constexpr QArea mile2 = mile * mile;
+} // namespace okapi

SerialTest/include/okapi/api/units/QForce.hpp

@@ -0,0 +1,43 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QAcceleration.hpp"
+#include "okapi/api/units/QMass.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(1, 1, -2, 0, QForce)
+
+constexpr QForce newton = (kg * meter) / (second * second);
+constexpr QForce poundforce = pound * G;
+constexpr QForce kilopond = kg * G;
+
+inline namespace literals {
+constexpr QForce operator"" _n(long double x) {
+  return QForce(x);
+}
+constexpr QForce operator"" _n(unsigned long long int x) {
+  return QForce(static_cast<double>(x));
+}
+constexpr QForce operator"" _lbf(long double x) {
+  return static_cast<double>(x) * poundforce;
+}
+constexpr QForce operator"" _lbf(unsigned long long int x) {
+  return static_cast<double>(x) * poundforce;
+}
+constexpr QForce operator"" _kp(long double x) {
+  return static_cast<double>(x) * kilopond;
+}
+constexpr QForce operator"" _kp(unsigned long long int x) {
+  return static_cast<double>(x) * kilopond;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QFrequency.hpp

@@ -0,0 +1,27 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, -1, 0, QFrequency)
+
+constexpr QFrequency Hz(1.0);
+
+inline namespace literals {
+constexpr QFrequency operator"" _Hz(long double x) {
+  return QFrequency(x);
+}
+constexpr QFrequency operator"" _Hz(unsigned long long int x) {
+  return QFrequency(static_cast<long double>(x));
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QJerk.hpp

@@ -0,0 +1,18 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 1, -3, 0, QJerk)
+}

SerialTest/include/okapi/api/units/QLength.hpp

@@ -0,0 +1,84 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 1, 0, 0, QLength)
+
+constexpr QLength meter(1.0); // SI base unit
+constexpr QLength decimeter = meter / 10;
+constexpr QLength centimeter = meter / 100;
+constexpr QLength millimeter = meter / 1000;
+constexpr QLength kilometer = 1000 * meter;
+constexpr QLength inch = 2.54 * centimeter;
+constexpr QLength foot = 12 * inch;
+constexpr QLength yard = 3 * foot;
+constexpr QLength mile = 5280 * foot;
+constexpr QLength tile = 24 * inch;
+
+inline namespace literals {
+constexpr QLength operator"" _mm(long double x) {
+  return static_cast<double>(x) * millimeter;
+}
+constexpr QLength operator"" _cm(long double x) {
+  return static_cast<double>(x) * centimeter;
+}
+constexpr QLength operator"" _m(long double x) {
+  return static_cast<double>(x) * meter;
+}
+constexpr QLength operator"" _km(long double x) {
+  return static_cast<double>(x) * kilometer;
+}
+constexpr QLength operator"" _mi(long double x) {
+  return static_cast<double>(x) * mile;
+}
+constexpr QLength operator"" _yd(long double x) {
+  return static_cast<double>(x) * yard;
+}
+constexpr QLength operator"" _ft(long double x) {
+  return static_cast<double>(x) * foot;
+}
+constexpr QLength operator"" _in(long double x) {
+  return static_cast<double>(x) * inch;
+}
+constexpr QLength operator"" _tile(long double x) {
+  return static_cast<double>(x) * tile;
+}
+constexpr QLength operator"" _mm(unsigned long long int x) {
+  return static_cast<double>(x) * millimeter;
+}
+constexpr QLength operator"" _cm(unsigned long long int x) {
+  return static_cast<double>(x) * centimeter;
+}
+constexpr QLength operator"" _m(unsigned long long int x) {
+  return static_cast<double>(x) * meter;
+}
+constexpr QLength operator"" _km(unsigned long long int x) {
+  return static_cast<double>(x) * kilometer;
+}
+constexpr QLength operator"" _mi(unsigned long long int x) {
+  return static_cast<double>(x) * mile;
+}
+constexpr QLength operator"" _yd(unsigned long long int x) {
+  return static_cast<double>(x) * yard;
+}
+constexpr QLength operator"" _ft(unsigned long long int x) {
+  return static_cast<double>(x) * foot;
+}
+constexpr QLength operator"" _in(unsigned long long int x) {
+  return static_cast<double>(x) * inch;
+}
+constexpr QLength operator"" _tile(unsigned long long int x) {
+  return static_cast<double>(x) * tile;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QMass.hpp

@@ -0,0 +1,62 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(1, 0, 0, 0, QMass)
+
+constexpr QMass kg(1.0); // SI base unit
+constexpr QMass gramme = 0.001 * kg;
+constexpr QMass tonne = 1000 * kg;
+constexpr QMass ounce = 0.028349523125 * kg;
+constexpr QMass pound = 16 * ounce;
+constexpr QMass stone = 14 * pound;
+
+inline namespace literals {
+constexpr QMass operator"" _kg(long double x) {
+  return QMass(x);
+}
+constexpr QMass operator"" _g(long double x) {
+  return static_cast<double>(x) * gramme;
+}
+constexpr QMass operator"" _t(long double x) {
+  return static_cast<double>(x) * tonne;
+}
+constexpr QMass operator"" _oz(long double x) {
+  return static_cast<double>(x) * ounce;
+}
+constexpr QMass operator"" _lb(long double x) {
+  return static_cast<double>(x) * pound;
+}
+constexpr QMass operator"" _st(long double x) {
+  return static_cast<double>(x) * stone;
+}
+constexpr QMass operator"" _kg(unsigned long long int x) {
+  return QMass(static_cast<double>(x));
+}
+constexpr QMass operator"" _g(unsigned long long int x) {
+  return static_cast<double>(x) * gramme;
+}
+constexpr QMass operator"" _t(unsigned long long int x) {
+  return static_cast<double>(x) * tonne;
+}
+constexpr QMass operator"" _oz(unsigned long long int x) {
+  return static_cast<double>(x) * ounce;
+}
+constexpr QMass operator"" _lb(unsigned long long int x) {
+  return static_cast<double>(x) * pound;
+}
+constexpr QMass operator"" _st(unsigned long long int x) {
+  return static_cast<double>(x) * stone;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QPressure.hpp

@@ -0,0 +1,44 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QAcceleration.hpp"
+#include "okapi/api/units/QArea.hpp"
+#include "okapi/api/units/QMass.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(1, -1, -2, 0, QPressure)
+
+constexpr QPressure pascal(1.0);
+constexpr QPressure bar = 100000 * pascal;
+constexpr QPressure psi = pound * G / inch2;
+
+inline namespace literals {
+constexpr QPressure operator"" _Pa(long double x) {
+  return QPressure(x);
+}
+constexpr QPressure operator"" _Pa(unsigned long long int x) {
+  return QPressure(static_cast<double>(x));
+}
+constexpr QPressure operator"" _bar(long double x) {
+  return static_cast<double>(x) * bar;
+}
+constexpr QPressure operator"" _bar(unsigned long long int x) {
+  return static_cast<double>(x) * bar;
+}
+constexpr QPressure operator"" _psi(long double x) {
+  return static_cast<double>(x) * psi;
+}
+constexpr QPressure operator"" _psi(unsigned long long int x) {
+  return static_cast<double>(x) * psi;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QSpeed.hpp

@@ -0,0 +1,43 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 1, -1, 0, QSpeed)
+
+constexpr QSpeed mps = meter / second;
+constexpr QSpeed miph = mile / hour;
+constexpr QSpeed kmph = kilometer / hour;
+
+inline namespace literals {
+constexpr QSpeed operator"" _mps(long double x) {
+  return static_cast<double>(x) * mps;
+}
+constexpr QSpeed operator"" _miph(long double x) {
+  return static_cast<double>(x) * mile / hour;
+}
+constexpr QSpeed operator"" _kmph(long double x) {
+  return static_cast<double>(x) * kilometer / hour;
+}
+constexpr QSpeed operator"" _mps(unsigned long long int x) {
+  return static_cast<double>(x) * mps;
+}
+constexpr QSpeed operator"" _miph(unsigned long long int x) {
+  return static_cast<double>(x) * mile / hour;
+}
+constexpr QSpeed operator"" _kmph(unsigned long long int x) {
+  return static_cast<double>(x) * kilometer / hour;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QTime.hpp

@@ -0,0 +1,55 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 0, 1, 0, QTime)
+
+constexpr QTime second(1.0); // SI base unit
+constexpr QTime millisecond = second / 1000;
+constexpr QTime minute = 60 * second;
+constexpr QTime hour = 60 * minute;
+constexpr QTime day = 24 * hour;
+
+inline namespace literals {
+constexpr QTime operator"" _s(long double x) {
+  return QTime(x);
+}
+constexpr QTime operator"" _ms(long double x) {
+  return static_cast<double>(x) * millisecond;
+}
+constexpr QTime operator"" _min(long double x) {
+  return static_cast<double>(x) * minute;
+}
+constexpr QTime operator"" _h(long double x) {
+  return static_cast<double>(x) * hour;
+}
+constexpr QTime operator"" _day(long double x) {
+  return static_cast<double>(x) * day;
+}
+constexpr QTime operator"" _s(unsigned long long int x) {
+  return QTime(static_cast<double>(x));
+}
+constexpr QTime operator"" _ms(unsigned long long int x) {
+  return static_cast<double>(x) * millisecond;
+}
+constexpr QTime operator"" _min(unsigned long long int x) {
+  return static_cast<double>(x) * minute;
+}
+constexpr QTime operator"" _h(unsigned long long int x) {
+  return static_cast<double>(x) * hour;
+}
+constexpr QTime operator"" _day(unsigned long long int x) {
+  return static_cast<double>(x) * day;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QTorque.hpp

@@ -0,0 +1,43 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QForce.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(1, 2, -2, 0, QTorque)
+
+constexpr QTorque newtonMeter = newton * meter;
+constexpr QTorque footPound = 1.355817948 * newtonMeter;
+constexpr QTorque inchPound = 0.083333333 * footPound;
+
+inline namespace literals {
+constexpr QTorque operator"" _nM(long double x) {
+  return QTorque(x);
+}
+constexpr QTorque operator"" _nM(unsigned long long int x) {
+  return QTorque(static_cast<double>(x));
+}
+constexpr QTorque operator"" _inLb(long double x) {
+  return static_cast<double>(x) * inchPound;
+}
+constexpr QTorque operator"" _inLb(unsigned long long int x) {
+  return static_cast<double>(x) * inchPound;
+}
+constexpr QTorque operator"" _ftLb(long double x) {
+  return static_cast<double>(x) * footPound;
+}
+constexpr QTorque operator"" _ftLb(unsigned long long int x) {
+  return static_cast<double>(x) * footPound;
+}
+} // namespace literals
+} // namespace okapi

SerialTest/include/okapi/api/units/QVolume.hpp

@@ -0,0 +1,28 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 "okapi/api/units/QArea.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/RQuantity.hpp"
+
+namespace okapi {
+QUANTITY_TYPE(0, 3, 0, 0, QVolume)
+
+constexpr QVolume kilometer3 = kilometer2 * kilometer;
+constexpr QVolume meter3 = meter2 * meter;
+constexpr QVolume decimeter3 = decimeter2 * decimeter;
+constexpr QVolume centimeter3 = centimeter2 * centimeter;
+constexpr QVolume millimeter3 = millimeter2 * millimeter;
+constexpr QVolume inch3 = inch2 * inch;
+constexpr QVolume foot3 = foot2 * foot;
+constexpr QVolume mile3 = mile2 * mile;
+constexpr QVolume litre = decimeter3;
+} // namespace okapi

SerialTest/include/okapi/api/units/RQuantity.hpp

@@ -0,0 +1,419 @@
+/*
+ * This code is a modified version of Benjamin Jurke's work in 2015. You can read his blog post
+ * here:
+ * https://benjaminjurke.com/content/articles/2015/compile-time-numerical-unit-dimension-checking/
+ *
+ * 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 <cmath>
+#include <ratio>
+
+namespace okapi {
+template <typename MassDim, typename LengthDim, typename TimeDim, typename AngleDim>
+class RQuantity {
+  public:
+  explicit constexpr RQuantity() : value(0.0) {
+  }
+
+  explicit constexpr RQuantity(double val) : value(val) {
+  }
+
+  explicit constexpr RQuantity(long double val) : value(static_cast<double>(val)) {
+  }
+
+  // The intrinsic operations for a quantity with a unit is addition and subtraction
+  constexpr RQuantity const &operator+=(const RQuantity &rhs) {
+    value += rhs.value;
+    return *this;
+  }
+
+  constexpr RQuantity const &operator-=(const RQuantity &rhs) {
+    value -= rhs.value;
+    return *this;
+  }
+
+  constexpr RQuantity operator-() {
+    return RQuantity(value * -1);
+  }
+
+  constexpr RQuantity const &operator*=(const double rhs) {
+    value *= rhs;
+    return *this;
+  }
+
+  constexpr RQuantity const &operator/=(const double rhs) {
+    value /= rhs;
+    return *this;
+  }
+
+  // Returns the value of the quantity in multiples of the specified unit
+  constexpr double convert(const RQuantity &rhs) const {
+    return value / rhs.value;
+  }
+
+  // returns the raw value of the quantity (should not be used)
+  constexpr double getValue() const {
+    return value;
+  }
+
+  constexpr RQuantity<MassDim, LengthDim, TimeDim, AngleDim> abs() const {
+    return RQuantity<MassDim, LengthDim, TimeDim, AngleDim>(std::fabs(value));
+  }
+
+  constexpr RQuantity<std::ratio_divide<MassDim, std::ratio<2>>,
+                      std::ratio_divide<LengthDim, std::ratio<2>>,
+                      std::ratio_divide<TimeDim, std::ratio<2>>,
+                      std::ratio_divide<AngleDim, std::ratio<2>>>
+  sqrt() const {
+    return RQuantity<std::ratio_divide<MassDim, std::ratio<2>>,
+                     std::ratio_divide<LengthDim, std::ratio<2>>,
+                     std::ratio_divide<TimeDim, std::ratio<2>>,
+                     std::ratio_divide<AngleDim, std::ratio<2>>>(std::sqrt(value));
+  }
+
+  private:
+  double value;
+};
+
+// Predefined (physical unit) quantity types:
+// ------------------------------------------
+#define QUANTITY_TYPE(_Mdim, _Ldim, _Tdim, _Adim, name)                                            \
+  typedef RQuantity<std::ratio<_Mdim>, std::ratio<_Ldim>, std::ratio<_Tdim>, std::ratio<_Adim>>    \
+    name;
+
+// Unitless
+QUANTITY_TYPE(0, 0, 0, 0, Number)
+constexpr Number number(1.0);
+
+// Standard arithmetic operators:
+// ------------------------------
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> operator+(const RQuantity<M, L, T, A> &lhs,
+                                          const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(lhs.getValue() + rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> operator-(const RQuantity<M, L, T, A> &lhs,
+                                          const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(lhs.getValue() - rhs.getValue());
+}
+template <typename M1,
+          typename L1,
+          typename T1,
+          typename A1,
+          typename M2,
+          typename L2,
+          typename T2,
+          typename A2>
+constexpr RQuantity<std::ratio_add<M1, M2>,
+                    std::ratio_add<L1, L2>,
+                    std::ratio_add<T1, T2>,
+                    std::ratio_add<A1, A2>>
+operator*(const RQuantity<M1, L1, T1, A1> &lhs, const RQuantity<M2, L2, T2, A2> &rhs) {
+  return RQuantity<std::ratio_add<M1, M2>,
+                   std::ratio_add<L1, L2>,
+                   std::ratio_add<T1, T2>,
+                   std::ratio_add<A1, A2>>(lhs.getValue() * rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> operator*(const double &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(lhs * rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> operator*(const RQuantity<M, L, T, A> &lhs, const double &rhs) {
+  return RQuantity<M, L, T, A>(lhs.getValue() * rhs);
+}
+template <typename M1,
+          typename L1,
+          typename T1,
+          typename A1,
+          typename M2,
+          typename L2,
+          typename T2,
+          typename A2>
+constexpr RQuantity<std::ratio_subtract<M1, M2>,
+                    std::ratio_subtract<L1, L2>,
+                    std::ratio_subtract<T1, T2>,
+                    std::ratio_subtract<A1, A2>>
+operator/(const RQuantity<M1, L1, T1, A1> &lhs, const RQuantity<M2, L2, T2, A2> &rhs) {
+  return RQuantity<std::ratio_subtract<M1, M2>,
+                   std::ratio_subtract<L1, L2>,
+                   std::ratio_subtract<T1, T2>,
+                   std::ratio_subtract<A1, A2>>(lhs.getValue() / rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_subtract<std::ratio<0>, M>,
+                    std::ratio_subtract<std::ratio<0>, L>,
+                    std::ratio_subtract<std::ratio<0>, T>,
+                    std::ratio_subtract<std::ratio<0>, A>>
+operator/(const double &x, const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio_subtract<std::ratio<0>, M>,
+                   std::ratio_subtract<std::ratio<0>, L>,
+                   std::ratio_subtract<std::ratio<0>, T>,
+                   std::ratio_subtract<std::ratio<0>, A>>(x / rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> operator/(const RQuantity<M, L, T, A> &rhs, const double &x) {
+  return RQuantity<M, L, T, A>(rhs.getValue() / x);
+}
+
+// Comparison operators for quantities:
+// ------------------------------------
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator==(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() == rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator!=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() != rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator<=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() <= rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator>=(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() >= rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator<(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() < rhs.getValue());
+}
+template <typename M, typename L, typename T, typename A>
+constexpr bool operator>(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return (lhs.getValue() > rhs.getValue());
+}
+
+// Common math functions:
+// ------------------------------
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> abs(const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::abs(rhs.getValue()));
+}
+
+template <typename R, typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_multiply<M, R>,
+                    std::ratio_multiply<L, R>,
+                    std::ratio_multiply<T, R>,
+                    std::ratio_multiply<A, R>>
+pow(const RQuantity<M, L, T, A> &lhs) {
+  return RQuantity<std::ratio_multiply<M, R>,
+                   std::ratio_multiply<L, R>,
+                   std::ratio_multiply<T, R>,
+                   std::ratio_multiply<A, R>>(std::pow(lhs.getValue(), double(R::num) / R::den));
+}
+
+template <int R, typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_multiply<M, std::ratio<R>>,
+                    std::ratio_multiply<L, std::ratio<R>>,
+                    std::ratio_multiply<T, std::ratio<R>>,
+                    std::ratio_multiply<A, std::ratio<R>>>
+pow(const RQuantity<M, L, T, A> &lhs) {
+  return RQuantity<std::ratio_multiply<M, std::ratio<R>>,
+                   std::ratio_multiply<L, std::ratio<R>>,
+                   std::ratio_multiply<T, std::ratio<R>>,
+                   std::ratio_multiply<A, std::ratio<R>>>(std::pow(lhs.getValue(), R));
+}
+
+template <int R, typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_divide<M, std::ratio<R>>,
+                    std::ratio_divide<L, std::ratio<R>>,
+                    std::ratio_divide<T, std::ratio<R>>,
+                    std::ratio_divide<A, std::ratio<R>>>
+root(const RQuantity<M, L, T, A> &lhs) {
+  return RQuantity<std::ratio_divide<M, std::ratio<R>>,
+                   std::ratio_divide<L, std::ratio<R>>,
+                   std::ratio_divide<T, std::ratio<R>>,
+                   std::ratio_divide<A, std::ratio<R>>>(std::pow(lhs.getValue(), 1.0 / R));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_divide<M, std::ratio<2>>,
+                    std::ratio_divide<L, std::ratio<2>>,
+                    std::ratio_divide<T, std::ratio<2>>,
+                    std::ratio_divide<A, std::ratio<2>>>
+sqrt(const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio_divide<M, std::ratio<2>>,
+                   std::ratio_divide<L, std::ratio<2>>,
+                   std::ratio_divide<T, std::ratio<2>>,
+                   std::ratio_divide<A, std::ratio<2>>>(std::sqrt(rhs.getValue()));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_divide<M, std::ratio<3>>,
+                    std::ratio_divide<L, std::ratio<3>>,
+                    std::ratio_divide<T, std::ratio<3>>,
+                    std::ratio_divide<A, std::ratio<3>>>
+cbrt(const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio_divide<M, std::ratio<3>>,
+                   std::ratio_divide<L, std::ratio<3>>,
+                   std::ratio_divide<T, std::ratio<3>>,
+                   std::ratio_divide<A, std::ratio<3>>>(std::cbrt(rhs.getValue()));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_multiply<M, std::ratio<2>>,
+                    std::ratio_multiply<L, std::ratio<2>>,
+                    std::ratio_multiply<T, std::ratio<2>>,
+                    std::ratio_multiply<A, std::ratio<2>>>
+square(const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio_multiply<M, std::ratio<2>>,
+                   std::ratio_multiply<L, std::ratio<2>>,
+                   std::ratio_multiply<T, std::ratio<2>>,
+                   std::ratio_multiply<A, std::ratio<2>>>(std::pow(rhs.getValue(), 2));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio_multiply<M, std::ratio<3>>,
+                    std::ratio_multiply<L, std::ratio<3>>,
+                    std::ratio_multiply<T, std::ratio<3>>,
+                    std::ratio_multiply<A, std::ratio<3>>>
+cube(const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio_multiply<M, std::ratio<3>>,
+                   std::ratio_multiply<L, std::ratio<3>>,
+                   std::ratio_multiply<T, std::ratio<3>>,
+                   std::ratio_multiply<A, std::ratio<3>>>(std::pow(rhs.getValue(), 3));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> hypot(const RQuantity<M, L, T, A> &lhs,
+                                      const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::hypot(lhs.getValue(), rhs.getValue()));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> mod(const RQuantity<M, L, T, A> &lhs,
+                                    const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::fmod(lhs.getValue(), rhs.getValue()));
+}
+
+template <typename M1,
+          typename L1,
+          typename T1,
+          typename A1,
+          typename M2,
+          typename L2,
+          typename T2,
+          typename A2>
+constexpr RQuantity<M1, L1, T1, A1> copysign(const RQuantity<M1, L1, T1, A1> &lhs,
+                                             const RQuantity<M2, L2, T2, A2> &rhs) {
+  return RQuantity<M1, L1, T1, A1>(std::copysign(lhs.getValue(), rhs.getValue()));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> ceil(const RQuantity<M, L, T, A> &lhs,
+                                     const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::ceil(lhs.getValue() / rhs.getValue()) * rhs.getValue());
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> floor(const RQuantity<M, L, T, A> &lhs,
+                                      const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::floor(lhs.getValue() / rhs.getValue()) * rhs.getValue());
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> trunc(const RQuantity<M, L, T, A> &lhs,
+                                      const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::trunc(lhs.getValue() / rhs.getValue()) * rhs.getValue());
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<M, L, T, A> round(const RQuantity<M, L, T, A> &lhs,
+                                      const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<M, L, T, A>(std::round(lhs.getValue() / rhs.getValue()) * rhs.getValue());
+}
+
+// Common trig functions:
+// ------------------------------
+
+constexpr Number
+sin(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::sin(rhs.getValue()));
+}
+
+constexpr Number
+cos(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::cos(rhs.getValue()));
+}
+
+constexpr Number
+tan(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::tan(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+asin(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::asin(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+acos(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::acos(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+atan(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::atan(rhs.getValue()));
+}
+
+constexpr Number
+sinh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::sinh(rhs.getValue()));
+}
+
+constexpr Number
+cosh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::cosh(rhs.getValue()));
+}
+
+constexpr Number
+tanh(const RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>> &rhs) {
+  return Number(std::tanh(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+asinh(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::asinh(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+acosh(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::acosh(rhs.getValue()));
+}
+
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+atanh(const Number &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::atanh(rhs.getValue()));
+}
+
+template <typename M, typename L, typename T, typename A>
+constexpr RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>
+atan2(const RQuantity<M, L, T, A> &lhs, const RQuantity<M, L, T, A> &rhs) {
+  return RQuantity<std::ratio<0>, std::ratio<0>, std::ratio<0>, std::ratio<1>>(
+    std::atan2(lhs.getValue(), rhs.getValue()));
+}
+
+inline namespace literals {
+constexpr long double operator"" _pi(long double x) {
+  return static_cast<double>(x) * 3.1415926535897932384626433832795;
+}
+constexpr long double operator"" _pi(unsigned long long int x) {
+  return static_cast<double>(x) * 3.1415926535897932384626433832795;
+}
+} // namespace literals
+} // namespace okapi
+
+// Conversion macro, which utilizes the string literals
+#define ConvertTo(_x, _y) (_x).convert(1.0_##_y)

SerialTest/include/okapi/api/units/RQuantityName.hpp

@@ -0,0 +1,46 @@
+#include "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+#include "okapi/api/units/QSpeed.hpp"
+#include <stdexcept>
+#include <typeindex>
+#include <unordered_map>
+
+#pragma once
+
+namespace okapi {
+
+/**
+* Returns a short name for a unit.
+* For example: `str(1_ft)` will return "ft", so will `1 * foot` or `0.3048_m`.
+* Throws std::domain_error when `q` is a unit not defined in this function.
+*
+* @param q Your unit. Currently only QLength and QAngle are supported.
+* @return The short string suffix for that unit.
+*/
+template <class QType> std::string getShortUnitName(QType q) {
+  const std::unordered_map<std::type_index, std::unordered_map<double, const char *>> shortNameMap =
+    {{typeid(meter),
+      {
+        {meter.getValue(), "m"},
+        {decimeter.getValue(), "dm"},
+        {centimeter.getValue(), "cm"},
+        {millimeter.getValue(), "mm"},
+        {kilometer.getValue(), "km"},
+        {inch.getValue(), "in"},
+        {foot.getValue(), "ft"},
+        {yard.getValue(), "yd"},
+        {mile.getValue(), "mi"},
+        {tile.getValue(), "tile"},
+      }},
+     {typeid(degree), {{degree.getValue(), "deg"}, {radian.getValue(), "rad"}}}};
+
+  try {
+    return shortNameMap.at(typeid(q)).at(q.getValue());
+  } catch (const std::out_of_range &e) {
+    throw std::domain_error(
+      "You have requested the shortname of an unknown unit somewhere (likely odometry strings). "
+      "Shortname for provided unit is unspecified. You can override this function to add more "
+      "names or manually specify the name instead.");
+  }
+}
+} // namespace okapi

SerialTest/include/okapi/api/util/abstractRate.hpp

@@ -0,0 +1,41 @@
+/*
+ * 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 "okapi/api/coreProsAPI.hpp"
+#include "okapi/api/units/QFrequency.hpp"
+#include "okapi/api/units/QTime.hpp"
+
+namespace okapi {
+class AbstractRate {
+  public:
+  virtual ~AbstractRate();
+
+  /**
+   * Delay the current task such that it runs at the given frequency. The first delay will run for
+   * 1000/(ihz). Subsequent delays will adjust according to the previous runtime of the task.
+   *
+   * @param ihz the frequency
+   */
+  virtual void delay(QFrequency ihz) = 0;
+
+  /**
+   * Delay the current task until itime has passed. This method can be used by periodic tasks to
+   * ensure a consistent execution frequency.
+   *
+   * @param itime the time period
+   */
+  virtual void delayUntil(QTime itime) = 0;
+
+  /**
+   * Delay the current task until ims milliseconds have passed. This method can be used by
+   * periodic tasks to ensure a consistent execution frequency.
+   *
+   * @param ims the time period
+   */
+  virtual void delayUntil(uint32_t ims) = 0;
+};
+} // namespace okapi

SerialTest/include/okapi/api/util/abstractTimer.hpp

@@ -0,0 +1,125 @@
+/*
+ * 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 "okapi/api/units/QFrequency.hpp"
+#include "okapi/api/units/QTime.hpp"
+
+namespace okapi {
+class AbstractTimer {
+  public:
+  /**
+   * A Timer base class which implements its methods in terms of millis().
+   *
+   * @param ifirstCalled the current time
+   */
+  explicit AbstractTimer(QTime ifirstCalled);
+
+  virtual ~AbstractTimer();
+
+  /**
+   * Returns the current time in units of QTime.
+   *
+   * @return the current time
+   */
+  virtual QTime millis() const = 0;
+
+  /**
+   * Returns the time passed in ms since the previous call of this function.
+   *
+   * @return The time passed in ms since the previous call of this function
+   */
+  virtual QTime getDt();
+
+  /**
+   * Returns the time passed in ms since the previous call of getDt(). Does not change the time
+   * recorded by getDt().
+   *
+   * @return The time passed in ms since the previous call of getDt()
+   */
+  virtual QTime readDt() const;
+
+  /**
+   * Returns the time the timer was first constructed.
+   *
+   * @return The time the timer was first constructed
+   */
+  virtual QTime getStartingTime() const;
+
+  /**
+   * Returns the time since the timer was first constructed.
+   *
+   * @return The time since the timer was first constructed
+   */
+  virtual QTime getDtFromStart() const;
+
+  /**
+   * Place a time marker. Placing another marker will overwrite the previous one.
+   */
+  virtual void placeMark();
+
+  /**
+   * Clears the marker.
+   *
+   * @return The old marker
+   */
+  virtual QTime clearMark();
+
+  /**
+   * Place a hard time marker. Placing another hard marker will not overwrite the previous one;
+   * instead, call clearHardMark() and then place another.
+   */
+  virtual void placeHardMark();
+
+  /**
+   * Clears the hard marker.
+   *
+   * @return The old hard marker
+   */
+  virtual QTime clearHardMark();
+
+  /**
+   * Returns the time since the time marker. Returns 0_ms if there is no marker.
+   *
+   * @return The time since the time marker
+   */
+  virtual QTime getDtFromMark() const;
+
+  /**
+   * Returns the time since the hard time marker. Returns 0_ms if there is no hard marker set.
+   *
+   * @return The time since the hard time marker
+   */
+  virtual QTime getDtFromHardMark() const;
+
+  /**
+   * Returns true when the input time period has passed, then resets. Meant to be used in loops
+   * to run an action every time period without blocking.
+   *
+   * @param time time period
+   * @return true when the input time period has passed, false after reading true until the
+   *   period has passed again
+   */
+  virtual bool repeat(QTime time);
+
+  /**
+   * Returns true when the input time period has passed, then resets. Meant to be used in loops
+   * to run an action every time period without blocking.
+   *
+   * @param frequency the repeat frequency
+   * @return true when the input time period has passed, false after reading true until the
+   *   period has passed again
+   */
+  virtual bool repeat(QFrequency frequency);
+
+  protected:
+  QTime firstCalled;
+  QTime lastCalled;
+  QTime mark;
+  QTime hardMark;
+  QTime repeatMark;
+};
+} // namespace okapi

SerialTest/include/okapi/api/util/logging.hpp

@@ -0,0 +1,192 @@
+/*
+ * 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 "okapi/api/coreProsAPI.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include "okapi/api/util/mathUtil.hpp"
+#include <memory>
+#include <mutex>
+
+#if defined(THREADS_STD)
+#else
+#include "okapi/impl/util/timer.hpp"
+#endif
+
+#define LOG_DEBUG(msg) logger->debug([=]() { return msg; })
+#define LOG_INFO(msg) logger->info([=]() { return msg; })
+#define LOG_WARN(msg) logger->warn([=]() { return msg; })
+#define LOG_ERROR(msg) logger->error([=]() { return msg; })
+
+#define LOG_DEBUG_S(msg) LOG_DEBUG(std::string(msg))
+#define LOG_INFO_S(msg) LOG_INFO(std::string(msg))
+#define LOG_WARN_S(msg) LOG_WARN(std::string(msg))
+#define LOG_ERROR_S(msg) LOG_ERROR(std::string(msg))
+
+namespace okapi {
+class Logger {
+  public:
+  enum class LogLevel {
+    debug = 4, ///< debug
+    info = 3,  ///< info
+    warn = 2,  ///< warn
+    error = 1, ///< error
+    off = 0    ///< off
+  };
+
+  /**
+   * A logger that does nothing.
+   */
+  Logger() noexcept;
+
+  /**
+   * A logger that opens the input file by name. If the file contains `/ser/`, the file will be
+   * opened in write mode. Otherwise, the file will be opened in append mode. The file will be
+   * closed when the logger is destructed.
+   *
+   * @param itimer A timer used to get the current time for log statements.
+   * @param ifileName The name of the log file to open.
+   * @param ilevel The log level. Log statements more verbose than this level will be disabled.
+   */
+  Logger(std::unique_ptr<AbstractTimer> itimer,
+         std::string_view ifileName,
+         const LogLevel &ilevel) noexcept;
+
+  /**
+   * A logger that uses an existing file handle. The file will be closed when the logger is
+   * destructed.
+   *
+   * @param itimer A timer used to get the current time for log statements.
+   * @param ifile The log file to open. Will be closed by the logger!
+   * @param ilevel The log level. Log statements more verbose than this level will be disabled.
+   */
+  Logger(std::unique_ptr<AbstractTimer> itimer, FILE *ifile, const LogLevel &ilevel) noexcept;
+
+  ~Logger();
+
+  constexpr bool isDebugLevelEnabled() const noexcept {
+    return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::debug);
+  }
+
+  template <typename T> void debug(T ilazyMessage) noexcept {
+    if (isDebugLevelEnabled() && logfile && timer) {
+      std::scoped_lock lock(logfileMutex);
+      fprintf(logfile,
+              "%ld (%s) DEBUG: %s\n",
+              static_cast<long>(timer->millis().convert(millisecond)),
+              CrossplatformThread::getName().c_str(),
+              ilazyMessage().c_str());
+    }
+  }
+
+  constexpr bool isInfoLevelEnabled() const noexcept {
+    return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::info);
+  }
+
+  template <typename T> void info(T ilazyMessage) noexcept {
+    if (isInfoLevelEnabled() && logfile && timer) {
+      std::scoped_lock lock(logfileMutex);
+      fprintf(logfile,
+              "%ld (%s) INFO: %s\n",
+              static_cast<long>(timer->millis().convert(millisecond)),
+              CrossplatformThread::getName().c_str(),
+              ilazyMessage().c_str());
+    }
+  }
+
+  constexpr bool isWarnLevelEnabled() const noexcept {
+    return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::warn);
+  }
+
+  template <typename T> void warn(T ilazyMessage) noexcept {
+    if (isWarnLevelEnabled() && logfile && timer) {
+      std::scoped_lock lock(logfileMutex);
+      fprintf(logfile,
+              "%ld (%s) WARN: %s\n",
+              static_cast<long>(timer->millis().convert(millisecond)),
+              CrossplatformThread::getName().c_str(),
+              ilazyMessage().c_str());
+    }
+  }
+
+  constexpr bool isErrorLevelEnabled() const noexcept {
+    return toUnderlyingType(logLevel) >= toUnderlyingType(LogLevel::error);
+  }
+
+  template <typename T> void error(T ilazyMessage) noexcept {
+    if (isErrorLevelEnabled() && logfile && timer) {
+      std::scoped_lock lock(logfileMutex);
+      fprintf(logfile,
+              "%ld (%s) ERROR: %s\n",
+              static_cast<long>(timer->millis().convert(millisecond)),
+              CrossplatformThread::getName().c_str(),
+              ilazyMessage().c_str());
+    }
+  }
+
+  /**
+   * Closes the connection to the log file.
+   */
+  constexpr void close() noexcept {
+    if (logfile) {
+      fclose(logfile);
+      logfile = nullptr;
+    }
+  }
+
+  /**
+   * @return The default logger.
+   */
+  static std::shared_ptr<Logger> getDefaultLogger();
+
+  /**
+   * Sets a new default logger. OkapiLib classes use the default logger unless given another logger
+   * in their constructor.
+   *
+   * @param ilogger The new logger instance.
+   */
+  static void setDefaultLogger(std::shared_ptr<Logger> ilogger);
+
+  private:
+  const std::unique_ptr<AbstractTimer> timer;
+  const LogLevel logLevel;
+  FILE *logfile;
+  CrossplatformMutex logfileMutex;
+
+  static bool isSerialStream(std::string_view filename);
+};
+
+extern std::shared_ptr<Logger> defaultLogger;
+
+struct DefaultLoggerInitializer {
+  DefaultLoggerInitializer() {
+    if (count++ == 0) {
+      init();
+    }
+  }
+  ~DefaultLoggerInitializer() {
+    if (--count == 0) {
+      cleanup();
+    }
+  }
+
+  static int count;
+
+  static void init() {
+#if defined(THREADS_STD)
+    defaultLogger = std::make_shared<Logger>();
+#else
+    defaultLogger =
+      std::make_shared<Logger>(std::make_unique<Timer>(), "/ser/sout", Logger::LogLevel::warn);
+#endif
+  }
+
+  static void cleanup() {
+  }
+};
+
+static DefaultLoggerInitializer defaultLoggerInitializer; // NOLINT(cert-err58-cpp)
+} // namespace okapi

SerialTest/include/okapi/api/util/mathUtil.hpp

@@ -0,0 +1,255 @@
+/*
+ * 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 "okapi/api/device/motor/abstractMotor.hpp"
+#include <algorithm>
+#include <cstdint>
+#include <math.h>
+#include <type_traits>
+
+namespace okapi {
+/**
+ * Converts inches to millimeters.
+ */
+static constexpr double inchToMM = 25.4;
+
+/**
+ * Converts millimeters to inches.
+ */
+static constexpr double mmToInch = 0.0393700787;
+
+/**
+ * Converts degrees to radians.
+ */
+static constexpr double degreeToRadian = 0.01745329252;
+
+/**
+ * Converts radians to degrees.
+ */
+static constexpr double radianToDegree = 57.2957795;
+
+/**
+ * The ticks per rotation of the 393 IME with torque gearing.
+ */
+static constexpr double imeTorqueTPR = 627.2;
+
+/**
+ * The ticks per rotation of the 393 IME with speed gearing.
+ */
+static constexpr std::int32_t imeSpeedTPR = 392;
+
+/**
+ * The ticks per rotation of the 393 IME with turbo gearing.
+ */
+static constexpr double imeTurboTPR = 261.333;
+
+/**
+ * The ticks per rotation of the 269 IME.
+ */
+static constexpr double ime269TPR = 240.448;
+
+/**
+ * The ticks per rotation of the V5 motor with a red gearset.
+ */
+static constexpr std::int32_t imev5RedTPR = 1800;
+
+/**
+ * The ticks per rotation of the V5 motor with a green gearset.
+ */
+static constexpr std::int32_t imev5GreenTPR = 900;
+
+/**
+ * The ticks per rotation of the V5 motor with a blue gearset.
+ */
+static constexpr std::int32_t imev5BlueTPR = 300;
+
+/**
+ * The ticks per rotation of the red quadrature encoders.
+ */
+static constexpr std::int32_t quadEncoderTPR = 360;
+
+/**
+ * The value of pi.
+ */
+static constexpr double pi = 3.1415926535897932;
+
+/**
+ * The value of pi divided by 2.
+ */
+static constexpr double pi2 = 1.5707963267948966;
+
+/**
+ * The conventional value of gravity of Earth.
+ */
+static constexpr double gravity = 9.80665;
+
+/**
+ * Same as PROS_ERR.
+ */
+static constexpr auto OKAPI_PROS_ERR = INT32_MAX;
+
+/**
+ * Same as PROS_ERR_F.
+ */
+static constexpr auto OKAPI_PROS_ERR_F = INFINITY;
+
+/**
+ * The maximum voltage that can be sent to V5 motors.
+ */
+static constexpr double v5MotorMaxVoltage = 12000;
+
+/**
+ * The polling frequency of V5 motors in milliseconds.
+ */
+static constexpr std::int8_t motorUpdateRate = 10;
+
+/**
+ * The polling frequency of the ADI ports in milliseconds.
+ */
+static constexpr std::int8_t adiUpdateRate = 10;
+
+/**
+ * Integer power function. Computes `base^expo`.
+ *
+ * @param base The base.
+ * @param expo The exponent.
+ * @return `base^expo`.
+ */
+constexpr double ipow(const double base, const int expo) {
+  return (expo == 0) ? 1
+         : expo == 1 ? base
+         : expo > 1  ? ((expo & 1) ? base * ipow(base, expo - 1)
+                                   : ipow(base, expo / 2) * ipow(base, expo / 2))
+                     : 1 / ipow(base, -expo);
+}
+
+/**
+ * Cuts out a range from the number. The new range of the input number will be
+ * `(-inf, min]U[max, +inf)`. If value sits equally between `min` and `max`, `max` will be returned.
+ *
+ * @param value The number to bound.
+ * @param min The lower bound of range.
+ * @param max The upper bound of range.
+ * @return The remapped value.
+ */
+constexpr double cutRange(const double value, const double min, const double max) {
+  const double middle = max - ((max - min) / 2);
+
+  if (value > min && value < middle) {
+    return min;
+  } else if (value <= max && value >= middle) {
+    return max;
+  }
+
+  return value;
+}
+
+/**
+ * Deadbands a range of the number. Returns the input value, or `0` if it is in the range `[min,
+ * max]`.
+ *
+ * @param value The number to deadband.
+ * @param min The lower bound of deadband.
+ * @param max The upper bound of deadband.
+ * @return The input value or `0` if it is in the range `[min, max]`.
+ */
+constexpr double deadband(const double value, const double min, const double max) {
+  return std::clamp(value, min, max) == value ? 0 : value;
+}
+
+/**
+ * Remap a value in the range `[oldMin, oldMax]` to the range `[newMin, newMax]`.
+ *
+ * @param value The value in the old range.
+ * @param oldMin The old range lower bound.
+ * @param oldMax The old range upper bound.
+ * @param newMin The new range lower bound.
+ * @param newMax The new range upper bound.
+ * @return The input value in the new range `[newMin, newMax]`.
+ */
+constexpr double remapRange(const double value,
+                            const double oldMin,
+                            const double oldMax,
+                            const double newMin,
+                            const double newMax) {
+  return (value - oldMin) * ((newMax - newMin) / (oldMax - oldMin)) + newMin;
+}
+
+/**
+ * Converts an enum to its value type.
+ *
+ * @param e The enum value.
+ * @return The corresponding value.
+ */
+template <typename E> constexpr auto toUnderlyingType(const E e) noexcept {
+  return static_cast<std::underlying_type_t<E>>(e);
+}
+
+/**
+ * Converts a bool to a sign.
+ *
+ * @param b The bool.
+ * @return True corresponds to `1` and false corresponds to `-1`.
+ */
+constexpr auto boolToSign(const bool b) noexcept {
+  return b ? 1 : -1;
+}
+
+/**
+ * Computes `lhs mod rhs` using Euclidean division. C's `%` symbol computes the remainder, not
+ * modulus.
+ *
+ * @param lhs The left-hand side.
+ * @param rhs The right-hand side.
+ * @return `lhs` mod `rhs`.
+ */
+constexpr long modulus(const long lhs, const long rhs) noexcept {
+  return ((lhs % rhs) + rhs) % rhs;
+}
+
+/**
+ * Converts a gearset to its TPR.
+ *
+ * @param igearset The gearset.
+ * @return The corresponding TPR.
+ */
+constexpr std::int32_t gearsetToTPR(const AbstractMotor::gearset igearset) noexcept {
+  switch (igearset) {
+  case AbstractMotor::gearset::red:
+    return imev5RedTPR;
+  case AbstractMotor::gearset::green:
+    return imev5GreenTPR;
+  case AbstractMotor::gearset::blue:
+  case AbstractMotor::gearset::invalid:
+  default:
+    return imev5BlueTPR;
+  }
+}
+
+/**
+ * Maps ADI port numbers/chars to numbers:
+ * ```
+ * when (port) {
+ *   in ['a', 'h'] -> [1, 8]
+ *   in ['A', 'H'] -> [1, 8]
+ *   else -> [1, 8]
+ * }
+ * ```
+ *
+ * @param port The ADI port number or char.
+ * @return An equivalent ADI port number.
+ */
+constexpr std::int8_t transformADIPort(const std::int8_t port) {
+  if (port >= 'a' && port <= 'h') {
+    return port - ('a' - 1);
+  } else if (port >= 'A' && port <= 'H') {
+    return port - ('A' - 1);
+  } else {
+    return port;
+  }
+}
+} // namespace okapi

SerialTest/include/okapi/api/util/supplier.hpp

@@ -0,0 +1,34 @@
+/*
+ * 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>
+
+namespace okapi {
+/**
+ * A supplier of instances of T.
+ *
+ * @tparam T the type to supply
+ */
+template <typename T> class Supplier {
+  public:
+  explicit Supplier(std::function<T(void)> ifunc) : func(ifunc) {
+  }
+
+  virtual ~Supplier() = default;
+
+  /**
+   * Get an instance of type T. This is usually a new instance, but it does not have to be.
+   * @return an instance of T
+   */
+  T get() const {
+    return func();
+  }
+
+  protected:
+  std::function<T(void)> func;
+};
+} // namespace okapi

SerialTest/include/okapi/api/util/timeUtil.hpp

@@ -0,0 +1,41 @@
+/*
+ * 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 "okapi/api/control/util/settledUtil.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include "okapi/api/util/supplier.hpp"
+
+namespace okapi {
+/**
+ * Utility class for holding an AbstractTimer, AbstractRate, and SettledUtil together in one
+ * class since they are commonly used together.
+ */
+class TimeUtil {
+  public:
+  TimeUtil(const Supplier<std::unique_ptr<AbstractTimer>> &itimerSupplier,
+           const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
+           const Supplier<std::unique_ptr<SettledUtil>> &isettledUtilSupplier);
+
+  std::unique_ptr<AbstractTimer> getTimer() const;
+
+  std::unique_ptr<AbstractRate> getRate() const;
+
+  std::unique_ptr<SettledUtil> getSettledUtil() const;
+
+  Supplier<std::unique_ptr<AbstractTimer>> getTimerSupplier() const;
+
+  Supplier<std::unique_ptr<AbstractRate>> getRateSupplier() const;
+
+  Supplier<std::unique_ptr<SettledUtil>> getSettledUtilSupplier() const;
+
+  protected:
+  Supplier<std::unique_ptr<AbstractTimer>> timerSupplier;
+  Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
+  Supplier<std::unique_ptr<SettledUtil>> settledUtilSupplier;
+};
+} // namespace okapi