Initial commit - test serial

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

@@ -0,0 +1,131 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/control/async/asyncController.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativeController.hpp"
+#include "okapi/api/util/abstractRate.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+
+namespace okapi {
+template <typename Input, typename Output> class ControllerRunner {
+  public:
+  /**
+   * A utility class that runs a closed-loop controller.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param ilogger The logger this instance will log to.
+   */
+  explicit ControllerRunner(const TimeUtil &itimeUtil,
+                            const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger())
+    : logger(ilogger), rate(itimeUtil.getRate()) {
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget, AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilSettled(const Input itarget,
+                                 IterativeController<Input, Output> &icontroller,
+                                 ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    while (!icontroller.isSettled()) {
+      ioutput.controllerSet(icontroller.getOutput());
+      rate->delayUntil(10_ms);
+    }
+
+    LOG_INFO("ControllerRunner: runUntilSettled(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  AsyncController<Input, Output> &icontroller) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(AsyncController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  /**
+   * Runs the controller until it has reached its target, but not necessarily settled.
+   *
+   * @param itarget the new target
+   * @param icontroller the controller to run
+   * @param ioutput the output to write to
+   * @return the error when settled
+   */
+  virtual Output runUntilAtTarget(const Input itarget,
+                                  IterativeController<Input, Output> &icontroller,
+                                  ControllerOutput<Output> &ioutput) {
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Set target to " +
+             std::to_string(itarget));
+    icontroller.setTarget(itarget);
+
+    double error = icontroller.getError();
+    double lastError = error;
+    while (error != 0 && std::copysign(1.0, error) == std::copysign(1.0, lastError)) {
+      ioutput.controllerSet(icontroller.getOutput());
+      lastError = error;
+      rate->delayUntil(10_ms);
+      error = icontroller.getError();
+    }
+
+    LOG_INFO("ControllerRunner: runUntilAtTarget(IterativeController): Done waiting to settle");
+    return icontroller.getError();
+  }
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::unique_ptr<AbstractRate> rate;
+};
+} // namespace okapi

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

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

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

@@ -0,0 +1,23 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/units/QAngle.hpp"
+#include "okapi/api/units/QLength.hpp"
+
+namespace okapi {
+struct PathfinderPoint {
+  QLength x;    // X coordinate relative to the start of the movement
+  QLength y;    // Y coordinate relative to the start of the movement
+  QAngle theta; // Exit angle relative to the start of the movement
+};
+
+struct PathfinderLimits {
+  double maxVel;   // Maximum robot velocity in m/s
+  double maxAccel; // Maximum robot acceleration in m/s/s
+  double maxJerk;  // Maximum robot jerk in m/s/s/s
+};
+} // namespace okapi

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

@@ -0,0 +1,80 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/control/controllerInput.hpp"
+#include "okapi/api/control/controllerOutput.hpp"
+#include "okapi/api/control/iterative/iterativePosPidController.hpp"
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+#include <memory>
+#include <vector>
+
+namespace okapi {
+class PIDTuner {
+  public:
+  struct Output {
+    double kP, kI, kD;
+  };
+
+  PIDTuner(const std::shared_ptr<ControllerInput<double>> &iinput,
+           const std::shared_ptr<ControllerOutput<double>> &ioutput,
+           const TimeUtil &itimeUtil,
+           QTime itimeout,
+           std::int32_t igoal,
+           double ikPMin,
+           double ikPMax,
+           double ikIMin,
+           double ikIMax,
+           double ikDMin,
+           double ikDMax,
+           std::size_t inumIterations = 5,
+           std::size_t inumParticles = 16,
+           double ikSettle = 1,
+           double ikITAE = 2,
+           const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
+
+  virtual ~PIDTuner();
+
+  virtual Output autotune();
+
+  protected:
+  static constexpr double inertia = 0.5;   // Particle inertia
+  static constexpr double confSelf = 1.1;  // Self confidence
+  static constexpr double confSwarm = 1.2; // Particle swarm confidence
+  static constexpr int increment = 5;
+  static constexpr int divisor = 5;
+  static constexpr QTime loopDelta = 10_ms; // NOLINT
+
+  struct Particle {
+    double pos, vel, best;
+  };
+
+  struct ParticleSet {
+    Particle kP, kI, kD;
+    double bestError;
+  };
+
+  std::shared_ptr<Logger> logger;
+  TimeUtil timeUtil;
+  std::shared_ptr<ControllerInput<double>> input;
+  std::shared_ptr<ControllerOutput<double>> output;
+
+  const QTime timeout;
+  const std::int32_t goal;
+  const double kPMin;
+  const double kPMax;
+  const double kIMin;
+  const double kIMax;
+  const double kDMin;
+  const double kDMax;
+  const std::size_t numIterations;
+  const std::size_t numParticles;
+  const double kSettle;
+  const double kITAE;
+};
+} // namespace okapi

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

@@ -0,0 +1,51 @@
+/*
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+#pragma once
+
+#include "okapi/api/units/QTime.hpp"
+#include "okapi/api/util/abstractTimer.hpp"
+#include <memory>
+
+namespace okapi {
+class SettledUtil {
+  public:
+  /**
+   * A utility class to determine if a control loop has settled based on error. A control loop is
+   * settled if the error is within `iatTargetError` and `iatTargetDerivative` for `iatTargetTime`.
+   *
+   * @param iatTargetTimer A timer used to track `iatTargetTime`.
+   * @param iatTargetError The minimum error to be considered settled.
+   * @param iatTargetDerivative The minimum error derivative to be considered settled.
+   * @param iatTargetTime The minimum time within atTargetError to be considered settled.
+   */
+  explicit SettledUtil(std::unique_ptr<AbstractTimer> iatTargetTimer,
+                       double iatTargetError = 50,
+                       double iatTargetDerivative = 5,
+                       QTime iatTargetTime = 250_ms);
+
+  virtual ~SettledUtil();
+
+  /**
+   * Returns whether the controller is settled.
+   *
+   * @param ierror The current error.
+   * @return Whether the controller is settled.
+   */
+  virtual bool isSettled(double ierror);
+
+  /**
+   * Resets the "at target" timer and clears the previous error.
+   */
+  virtual void reset();
+
+  protected:
+  double atTargetError = 50;
+  double atTargetDerivative = 5;
+  QTime atTargetTime = 250_ms;
+  std::unique_ptr<AbstractTimer> atTargetTimer;
+  double lastError = 0;
+};
+} // namespace okapi