Initial commit - test serial

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

@@ -0,0 +1,95 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/odometry/odomState.hpp"
+#include "okapi/api/odometry/point.hpp"
+#include "okapi/api/util/logging.hpp"
+#include <tuple>
+
+namespace okapi {
+class OdomMath {
+  public:
+  /**
+   * Computes the distance from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance between the Odometry state and the point.
+   */
+  static QLength computeDistanceToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the angle from the given Odometry state to the given point. The point and the
+   * OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The angle between the Odometry state and the point.
+   */
+  static QAngle computeAngleToPoint(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance and angle from the given Odometry state to the given point. The point and
+   * the OdomState must be in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The distance and angle between the Odometry state and the point.
+   */
+  static std::pair<QLength, QAngle> computeDistanceAndAngleToPoint(const Point &ipoint,
+                                                                   const OdomState &istate);
+
+  /**
+   * Constraints the angle to [0,360] degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [0,360] degrees.
+   */
+  static QAngle constrainAngle360(const QAngle &angle);
+
+  /**
+   * Constraints the angle to [-180,180) degrees.
+   *
+   * @param angle The input angle.
+   * @return The angle normalized to [-180,180) degrees.
+   */
+  static QAngle constrainAngle180(const QAngle &angle);
+
+  private:
+  OdomMath();
+  ~OdomMath();
+
+  /**
+   * Computes the x and y diffs in meters between the points.
+   *
+   * @param ipoint The point.
+   * @param istate The Odometry state.
+   * @return The diffs in the order `{xDiff, yDiff}`.
+   */
+  static std::pair<double, double> computeDiffs(const Point &ipoint, const OdomState &istate);
+
+  /**
+   * Computes the distance between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @return The cartesian distance in meters.
+   */
+  static double computeDistance(double xDiff, double yDiff);
+
+  /**
+   * Compites the angle between the points.
+   *
+   * @param xDiff The x-axis diff in meters.
+   * @param yDiff The y-axis diff in meters.
+   * @param theta The current robot's theta in radians.
+   * @return The angle in radians.
+   */
+  static double computeAngle(double xDiff, double yDiff, double theta);
+};
+} // namespace okapi

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

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

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

@@ -0,0 +1,61 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/chassis/controller/chassisScales.hpp"
+#include "okapi/api/chassis/model/readOnlyChassisModel.hpp"
+#include "okapi/api/odometry/odomState.hpp"
+#include "okapi/api/odometry/stateMode.hpp"
+
+namespace okapi {
+class Odometry {
+  public:
+  /**
+   * Odometry. Tracks the movement of the robot and estimates its position in coordinates
+   * relative to the start (assumed to be (0, 0, 0)).
+   */
+  explicit Odometry() = default;
+
+  virtual ~Odometry() = default;
+
+  /**
+   * Sets the drive and turn scales.
+   */
+  virtual void setScales(const ChassisScales &ichassisScales) = 0;
+
+  /**
+   * Do one odometry step.
+   */
+  virtual void step() = 0;
+
+  /**
+   * Returns the current state.
+   *
+   * @param imode The mode to return the state in.
+   * @return The current state in the given format.
+   */
+  virtual OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const = 0;
+
+  /**
+   * Sets a new state to be the current state.
+   *
+   * @param istate The new state in the given format.
+   * @param imode The mode to treat the input state as.
+   */
+  virtual void setState(const OdomState &istate,
+                        const StateMode &imode = StateMode::FRAME_TRANSFORMATION) = 0;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  virtual std::shared_ptr<ReadOnlyChassisModel> getModel() = 0;
+
+  /**
+   * @return The internal ChassisScales.
+   */
+  virtual ChassisScales getScales() = 0;
+};
+} // namespace okapi

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

@@ -0,0 +1,30 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/odometry/stateMode.hpp"
+#include "okapi/api/units/QLength.hpp"
+
+namespace okapi {
+struct Point {
+  QLength x{0_m};
+  QLength y{0_m};
+
+  /**
+   * Computes the value of this point in `StateMode::FRAME_TRANSFORMATION`.
+   *
+   * @param imode The StateMode this Point is currently specified in.
+   * @return This point specified in `StateMode::FRAME_TRANSFORMATION`.
+   */
+  Point inFT(const StateMode &imode) const {
+    if (imode == StateMode::FRAME_TRANSFORMATION) {
+      return *this;
+    } else {
+      return {y, x};
+    }
+  }
+};
+} // namespace okapi

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

@@ -0,0 +1,17 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+namespace okapi {
+/**
+ * The mode for the OdomState calculated by Odometry.
+ */
+enum class StateMode {
+  FRAME_TRANSFORMATION, ///< +x is forward, +y is right, 0 degrees is along +x
+  CARTESIAN             ///< +x is right, +y is forward, 0 degrees is along +y
+};
+
+} // namespace okapi

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

@@ -0,0 +1,43 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/chassis/model/threeEncoderSkidSteerModel.hpp"
+#include "okapi/api/odometry/twoEncoderOdometry.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <functional>
+
+namespace okapi {
+class ThreeEncoderOdometry : public TwoEncoderOdometry {
+  public:
+  /**
+   * Odometry. Tracks the movement of the robot and estimates its position in coordinates
+   * relative to the start (assumed to be (0, 0)).
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The chassis model for reading sensors.
+   * @param ichassisScales See ChassisScales docs (the middle wheel scale is the third member)
+   * @param iwheelVelDelta The maximum delta between wheel velocities to consider the robot as
+   * driving straight.
+   * @param ilogger The logger this instance will log to.
+   */
+  ThreeEncoderOdometry(const TimeUtil &itimeUtil,
+                       const std::shared_ptr<ReadOnlyChassisModel> &imodel,
+                       const ChassisScales &ichassisScales,
+                       const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  protected:
+  /**
+   * Does the math, side-effect free, for one odom step.
+   *
+   * @param itickDiff The tick difference from the previous step to this step.
+   * @param ideltaT The time difference from the previous step to this step.
+   * @return The newly computed OdomState.
+   */
+  OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
+                         const QTime &ideltaT) override;
+};
+} // namespace okapi

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

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