Initial commit - test serial

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

@@ -0,0 +1,41 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/coreProsAPI.hpp"
+#include "okapi/api/units/QFrequency.hpp"
+#include "okapi/api/units/QTime.hpp"
+
+namespace okapi {
+class AbstractRate {
+  public:
+  virtual ~AbstractRate();
+
+  /**
+   * Delay the current task such that it runs at the given frequency. The first delay will run for
+   * 1000/(ihz). Subsequent delays will adjust according to the previous runtime of the task.
+   *
+   * @param ihz the frequency
+   */
+  virtual void delay(QFrequency ihz) = 0;
+
+  /**
+   * Delay the current task until itime has passed. This method can be used by periodic tasks to
+   * ensure a consistent execution frequency.
+   *
+   * @param itime the time period
+   */
+  virtual void delayUntil(QTime itime) = 0;
+
+  /**
+   * Delay the current task until ims milliseconds have passed. This method can be used by
+   * periodic tasks to ensure a consistent execution frequency.
+   *
+   * @param ims the time period
+   */
+  virtual void delayUntil(uint32_t ims) = 0;
+};
+} // namespace okapi

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

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

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

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

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

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

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

@@ -0,0 +1,34 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include <functional>
+
+namespace okapi {
+/**
+ * A supplier of instances of T.
+ *
+ * @tparam T the type to supply
+ */
+template <typename T> class Supplier {
+  public:
+  explicit Supplier(std::function<T(void)> ifunc) : func(ifunc) {
+  }
+
+  virtual ~Supplier() = default;
+
+  /**
+   * Get an instance of type T. This is usually a new instance, but it does not have to be.
+   * @return an instance of T
+   */
+  T get() const {
+    return func();
+  }
+
+  protected:
+  std::function<T(void)> func;
+};
+} // namespace okapi

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

@@ -0,0 +1,41 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/control/util/settledUtil.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include "okapi/api/util/supplier.hpp"
+
+namespace okapi {
+/**
+ * Utility class for holding an AbstractTimer, AbstractRate, and SettledUtil together in one
+ * class since they are commonly used together.
+ */
+class TimeUtil {
+  public:
+  TimeUtil(const Supplier<std::unique_ptr<AbstractTimer>> &itimerSupplier,
+           const Supplier<std::unique_ptr<AbstractRate>> &irateSupplier,
+           const Supplier<std::unique_ptr<SettledUtil>> &isettledUtilSupplier);
+
+  std::unique_ptr<AbstractTimer> getTimer() const;
+
+  std::unique_ptr<AbstractRate> getRate() const;
+
+  std::unique_ptr<SettledUtil> getSettledUtil() const;
+
+  Supplier<std::unique_ptr<AbstractTimer>> getTimerSupplier() const;
+
+  Supplier<std::unique_ptr<AbstractRate>> getRateSupplier() const;
+
+  Supplier<std::unique_ptr<SettledUtil>> getSettledUtilSupplier() const;
+
+  protected:
+  Supplier<std::unique_ptr<AbstractTimer>> timerSupplier;
+  Supplier<std::unique_ptr<AbstractRate>> rateSupplier;
+  Supplier<std::unique_ptr<SettledUtil>> settledUtilSupplier;
+};
+} // namespace okapi