Refactored everything.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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