chassisControllerPid.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/chassisController.hpp"
#include "okapi/api/control/iterative/iterativePosPidController.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <tuple>
 
namespace okapi {
class ChassisControllerPID : public ChassisController {
  public:
  ChassisControllerPID(
    TimeUtil itimeUtil,
    std::shared_ptr<ChassisModel> imodel,
    std::unique_ptr<IterativePosPIDController> idistanceController,
    std::unique_ptr<IterativePosPIDController> iturnController,
    std::unique_ptr<IterativePosPIDController> iangleController,
    const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
    const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
 
  ChassisControllerPID(const ChassisControllerPID &) = delete;
  ChassisControllerPID(ChassisControllerPID &&other) = delete;
  ChassisControllerPID &operator=(const ChassisControllerPID &other) = delete;
  ChassisControllerPID &operator=(ChassisControllerPID &&other) = delete;
 
  ~ChassisControllerPID() override;
 
  void moveDistance(QLength itarget) override;
 
  void moveRaw(double itarget) override;
 
  void moveDistanceAsync(QLength itarget) override;
 
  void moveRawAsync(double itarget) override;
 
  void turnAngle(QAngle idegTarget) override;
 
  void turnRaw(double idegTarget) override;
 
  void turnAngleAsync(QAngle idegTarget) override;
 
  void turnRawAsync(double idegTarget) override;
 
  void setTurnsMirrored(bool ishouldMirror) override;
 
  bool isSettled() override;
 
  void waitUntilSettled() override;
 
  ChassisScales getChassisScales() const override;
 
  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
 
  void setVelocityMode(bool ivelocityMode);
 
  void setGains(const IterativePosPIDController::Gains &idistanceGains,
                const IterativePosPIDController::Gains &iturnGains,
                const IterativePosPIDController::Gains &iangleGains);
 
  std::tuple<IterativePosPIDController::Gains,
             IterativePosPIDController::Gains,
             IterativePosPIDController::Gains>
  getGains() const;
 
  void startThread();
 
  CrossplatformThread *getThread() const;
 
  void stop() override;
 
  void setMaxVelocity(double imaxVelocity) override;
 
  double getMaxVelocity() const override;
 
  std::shared_ptr<ChassisModel> getModel() override;
 
  ChassisModel &model() override;
 
  protected:
  std::shared_ptr<Logger> logger;
  bool normalTurns{true};
  std::shared_ptr<ChassisModel> chassisModel;
  TimeUtil timeUtil;
  std::unique_ptr<IterativePosPIDController> distancePid;
  std::unique_ptr<IterativePosPIDController> turnPid;
  std::unique_ptr<IterativePosPIDController> anglePid;
  ChassisScales scales;
  AbstractMotor::GearsetRatioPair gearsetRatioPair;
  bool velocityMode{true};
  std::atomic_bool doneLooping{true};
  std::atomic_bool doneLoopingSeen{true};
  std::atomic_bool newMovement{false};
  std::atomic_bool dtorCalled{false};
  QTime threadSleepTime{10_ms};
 
  static void trampoline(void *context);
  void loop();
 
  bool waitForDistanceSettled();
 
  bool waitForAngleSettled();
 
  void stopAfterSettled();
 
  typedef enum { distance, angle, none } modeType;
  modeType mode{none};
 
  CrossplatformThread *task{nullptr};
};
} // namespace okapi