Initial commit

include/okapi/api/chassis/controller/chassisControllerIntegrated.hpp

@@ -0,0 +1,184 @@
+/*
+ * 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/async/asyncPosIntegratedController.hpp"
+#include "okapi/api/util/logging.hpp"
+#include "okapi/api/util/timeUtil.hpp"
+
+namespace okapi {
+class ChassisControllerIntegrated : public ChassisController {
+  public:
+  /**
+   * ChassisController using the V5 motor's integrated control. Puts the motors into encoder count
+   * units. Throws a `std::invalid_argument` exception if the gear ratio is zero. The initial
+   * model's max velocity will be propagated to the controllers.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param imodel The ChassisModel used to read from sensors/write to motors.
+   * @param ileftController The controller used for the left side motors.
+   * @param irightController The controller used for the right side motors.
+   * @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.
+   */
+  ChassisControllerIntegrated(
+    const TimeUtil &itimeUtil,
+    std::shared_ptr<ChassisModel> imodel,
+    std::unique_ptr<AsyncPosIntegratedController> ileftController,
+    std::unique_ptr<AsyncPosIntegratedController> irightController,
+    const AbstractMotor::GearsetRatioPair &igearset = AbstractMotor::gearset::green,
+    const ChassisScales &iscales = ChassisScales({1, 1}, imev5GreenTPR),
+    std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  /**
+   * 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;
+
+  /**
+   * Interrupts the current movement to stop the robot.
+   */
+  void stop() override;
+
+  /**
+   * Get the ChassisScales.
+   */
+  ChassisScales getChassisScales() const override;
+
+  /**
+   * Get the GearsetRatioPair.
+   */
+  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  std::shared_ptr<ChassisModel> getModel() override;
+
+  /**
+   * @return The internal ChassisModel.
+   */
+  ChassisModel &model() override;
+
+  /**
+   * Sets a new maximum velocity in RPM [0-600].
+   *
+   * @param imaxVelocity The new maximum velocity.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * @return The maximum velocity in RPM [0-600].
+   */
+  double getMaxVelocity() const override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  bool normalTurns{true};
+  std::shared_ptr<ChassisModel> chassisModel;
+  TimeUtil timeUtil;
+  std::unique_ptr<AsyncPosIntegratedController> leftController;
+  std::unique_ptr<AsyncPosIntegratedController> rightController;
+  int lastTarget;
+  ChassisScales scales;
+  AbstractMotor::GearsetRatioPair gearsetRatioPair;
+};
+} // namespace okapi