asyncMotionProfileController.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/chassis/controller/chassisScales.hpp"
#include "okapi/api/chassis/model/skidSteerModel.hpp"
#include "okapi/api/control/async/asyncPositionController.hpp"
#include "okapi/api/control/util/pathfinderUtil.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 <iostream>
#include <map>
 
#include "squiggles.hpp"
 
namespace okapi {
class AsyncMotionProfileController : public AsyncPositionController<std::string, PathfinderPoint> {
  public:
  AsyncMotionProfileController(const TimeUtil &itimeUtil,
                               const PathfinderLimits &ilimits,
                               const std::shared_ptr<ChassisModel> &imodel,
                               const ChassisScales &iscales,
                               const AbstractMotor::GearsetRatioPair &ipair,
                               const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
 
  AsyncMotionProfileController(AsyncMotionProfileController &&other) = delete;
 
  AsyncMotionProfileController &operator=(AsyncMotionProfileController &&other) = delete;
 
  ~AsyncMotionProfileController() override;
 
  void generatePath(std::initializer_list<PathfinderPoint> iwaypoints, const std::string &ipathId);
 
  void generatePath(std::initializer_list<PathfinderPoint> 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, bool imirrored = false);
 
  void controllerSet(std::string ivalue) override;
 
  std::string getTarget() override;
 
  std::string getProcessValue() const override;
 
  void waitUntilSettled() override;
 
  void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
              bool ibackwards = false,
              bool imirrored = false);
 
  void moveTo(std::initializer_list<PathfinderPoint> iwaypoints,
              const PathfinderLimits &ilimits,
              bool ibackwards = false,
              bool imirrored = false);
 
  PathfinderPoint 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 storePath(const std::string &idirectory, const std::string &ipathId);
 
  void loadPath(const std::string &idirectory, const std::string &ipathId);
 
  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<ChassisModel> model;
  ChassisScales scales;
  AbstractMotor::GearsetRatioPair pair;
  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 mirrored{false};
  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);
 
  static std::string makeFilePath(const std::string &directory, const std::string &filename);
 
  void internalStorePath(std::ostream &file, const std::string &ipathId);
  void internalLoadPath(std::istream &file, const std::string &ipathId);
  void internalLoadPathfinderPath(std::istream &leftFile,
                                  std::istream &rightFile,
                                  const std::string &ipathId);
 
  static constexpr double DT = 0.01;
};
} // namespace okapi