Initial commit

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