asyncLinearMotionProfileController.hpp

Go to the documentation of this file.

/*
 * 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/control/util/pathfinderUtil.hpp"
#include "okapi/api/device/motor/abstractMotor.hpp"
#include "okapi/api/units/QAngularSpeed.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <map>
 
#include "squiggles.hpp"
 
namespace okapi {
class AsyncLinearMotionProfileController : public AsyncPositionController<std::string, double> {
  public:
  AsyncLinearMotionProfileController(
    const TimeUtil &itimeUtil,
    const PathfinderLimits &ilimits,
    const std::shared_ptr<ControllerOutput<double>> &ioutput,
    const QLength &idiameter,
    const AbstractMotor::GearsetRatioPair &ipair,
    const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
 
  AsyncLinearMotionProfileController(AsyncLinearMotionProfileController &&other) = delete;
 
  AsyncLinearMotionProfileController &
  operator=(AsyncLinearMotionProfileController &&other) = delete;
 
  ~AsyncLinearMotionProfileController() override;
 
  void generatePath(std::initializer_list<QLength> iwaypoints, const std::string &ipathId);
 
  void generatePath(std::initializer_list<QLength> iwaypoints,
                    const std::string &ipathId,
                    const PathfinderLimits &ilimits);
 
  bool removePath(const std::string &ipathId);
 
  std::vector<std::string> getPaths();
 
  void setTarget(std::string ipathId) override;
 
  void setTarget(std::string ipathId, bool ibackwards);
 
  void controllerSet(std::string ivalue) override;
 
  std::string getTarget() override;
 
  virtual std::string getTarget() const;
 
  std::string getProcessValue() const override;
 
  void waitUntilSettled() override;
 
  void moveTo(const QLength &iposition, const QLength &itarget, bool ibackwards = false);
 
  void moveTo(const QLength &iposition,
              const QLength &itarget,
              const PathfinderLimits &ilimits,
              bool ibackwards = false);
 
  double getError() const override;
 
  bool isSettled() override;
 
  void reset() override;
 
  void flipDisable() override;
 
  void flipDisable(bool iisDisabled) override;
 
  bool isDisabled() const override;
 
  void tarePosition() override;
 
  void setMaxVelocity(std::int32_t imaxVelocity) override;
 
  void startThread();
 
  CrossplatformThread *getThread() const;
 
  void forceRemovePath(const std::string &ipathId);
 
  protected:
  std::shared_ptr<Logger> logger;
  std::map<std::string, std::vector<squiggles::ProfilePoint>> paths{};
  PathfinderLimits limits;
  std::shared_ptr<ControllerOutput<double>> output;
  QLength diameter;
  AbstractMotor::GearsetRatioPair pair;
  double currentProfilePosition{0};
  TimeUtil timeUtil;
 
  // This must be locked when accessing the current path
  CrossplatformMutex currentPathMutex;
 
  std::string currentPath{""};
  std::atomic_bool isRunning{false};
  std::atomic_int direction{1};
  std::atomic_bool disabled{false};
  std::atomic_bool dtorCalled{false};
  CrossplatformThread *task{nullptr};
 
  static void trampoline(void *context);
  void loop();
 
  virtual void executeSinglePath(const std::vector<squiggles::ProfilePoint> &path,
                                 std::unique_ptr<AbstractRate> rate);
 
  QAngularSpeed convertLinearToRotational(QSpeed linear) const;
 
  std::string getPathErrorMessage(const std::vector<PathfinderPoint> &points,
                                  const std::string &ipathId,
                                  int length);
};
} // namespace okapi