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