steak/include/okapi/api/chassis/controller/chassisControllerPid.hpp

/*
 * 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:
  /**
   * ChassisController using PID control. Puts the motors into encoder count units. Throws a
   * `std::invalid_argument` exception if the gear ratio is zero.
   *
   * @param itimeUtil The TimeUtil.
   * @param imodel The ChassisModel used to read from sensors/write to motors.
   * @param idistanceController The PID controller that controls chassis distance for driving
   * straight.
   * @param iturnController The PID controller that controls chassis angle for turning.
   * @param iangleController The PID controller that controls chassis angle for driving straight.
   * @param igearset The internal gearset and external ratio used on the drive motors.
   * @param iscales The ChassisScales.
   * @param ilogger The logger this instance will log to.
   */
  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;

  /**
   * Drives the robot straight for a distance (using closed-loop control).
   *
   * ```cpp
   * // Drive forward 6 inches
   * chassis->moveDistance(6_in);
   *
   * // Drive backward 0.2 meters
   * chassis->moveDistance(-0.2_m);
   * ```
   *
   * @param itarget distance to travel
   */
  void moveDistance(QLength itarget) override;

  /**
   * Drives the robot straight for a distance (using closed-loop control).
   *
   * ```cpp
   * // Drive forward by spinning the motors 400 degrees
   * chassis->moveRaw(400);
   * ```
   *
   * @param itarget distance to travel in motor degrees
   */
  void moveRaw(double itarget) override;

  /**
   * Sets the target distance for the robot to drive straight (using closed-loop control).
   *
   * @param itarget distance to travel
   */
  void moveDistanceAsync(QLength itarget) override;

  /**
   * Sets the target distance for the robot to drive straight (using closed-loop control).
   *
   * @param itarget distance to travel in motor degrees
   */
  void moveRawAsync(double itarget) override;

  /**
   * Turns the robot clockwise in place (using closed-loop control).
   *
   * ```cpp
   * // Turn 90 degrees clockwise
   * chassis->turnAngle(90_deg);
   * ```
   *
   * @param idegTarget angle to turn for
   */
  void turnAngle(QAngle idegTarget) override;

  /**
   * Turns the robot clockwise in place (using closed-loop control).
   *
   * ```cpp
   * // Turn clockwise by spinning the motors 200 degrees
   * chassis->turnRaw(200);
   * ```
   *
   * @param idegTarget angle to turn for in motor degrees
   */
  void turnRaw(double idegTarget) override;

  /**
   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
   *
   * @param idegTarget angle to turn for
   */
  void turnAngleAsync(QAngle idegTarget) override;

  /**
   * Sets the target angle for the robot to turn clockwise in place (using closed-loop control).
   *
   * @param idegTarget angle to turn for in motor degrees
   */
  void turnRawAsync(double idegTarget) override;

  /**
   * Sets whether turns should be mirrored.
   *
   * @param ishouldMirror whether turns should be mirrored
   */
  void setTurnsMirrored(bool ishouldMirror) override;

  /**
   * Checks whether the internal controllers are currently settled.
   *
   * @return Whether this ChassisController is settled.
   */
  bool isSettled() override;

  /**
   * Delays until the currently executing movement completes.
   */
  void waitUntilSettled() override;

  /**
   * Gets the ChassisScales.
   */
  ChassisScales getChassisScales() const override;

  /**
   * Gets the GearsetRatioPair.
   */
  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;

  /**
   * Sets the velocity mode flag. When the controller is in velocity mode, the control loop will
   * set motor velocities. When the controller is in voltage mode (`ivelocityMode = false`), the
   * control loop will set motor voltages. Additionally, when the controller is in voltage mode,
   * it will not obey maximum velocity limits.
   *
   * @param ivelocityMode Whether the controller should be in velocity or voltage mode.
   */
  void setVelocityMode(bool ivelocityMode);

  /**
   * Sets the gains for all controllers.
   *
   * @param idistanceGains The distance controller gains.
   * @param iturnGains The turn controller gains.
   * @param iangleGains The angle controller gains.
   */
  void setGains(const IterativePosPIDController::Gains &idistanceGains,
                const IterativePosPIDController::Gains &iturnGains,
                const IterativePosPIDController::Gains &iangleGains);

  /**
   * Gets the current controller gains.
   *
   * @return The current controller gains in the order: distance, turn, angle.
   */
  std::tuple<IterativePosPIDController::Gains,
             IterativePosPIDController::Gains,
             IterativePosPIDController::Gains>
  getGains() const;

  /**
   * Starts the internal thread. This method is called by the ChassisControllerBuilder when making a
   * new instance of this class.
   */
  void startThread();

  /**
   * Returns the underlying thread handle.
   *
   * @return The underlying thread handle.
   */
  CrossplatformThread *getThread() const;

  /**
   * Interrupts the current movement to stop the robot.
   */
  void stop() override;

  /**
   * Sets a new maximum velocity in RPM [0-600]. In voltage mode, the max velocity is ignored and a
   * max voltage should be set on the underlying ChassisModel instead.
   *
   * @param imaxVelocity The new maximum velocity.
   */
  void setMaxVelocity(double imaxVelocity) override;

  /**
   * @return The maximum velocity in RPM [0-600].
   */
  double getMaxVelocity() const override;

  /**
   * @return The internal ChassisModel.
   */
  std::shared_ptr<ChassisModel> getModel() override;

  /**
   * @return The internal ChassisModel.
   */
  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();

  /**
   * Wait for the distance setup (distancePid and anglePid) to settle.
   *
   * @return true if done settling; false if settling should be tried again
   */
  bool waitForDistanceSettled();

  /**
   * Wait for the angle setup (anglePid) to settle.
   *
   * @return true if done settling; false if settling should be tried again
   */
  bool waitForAngleSettled();

  /**
   * Stops all the controllers and the ChassisModel.
   */
  void stopAfterSettled();

  typedef enum { distance, angle, none } modeType;
  modeType mode{none};

  CrossplatformThread *task{nullptr};
};
} // namespace okapi