Initial commit

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

@@ -0,0 +1,183 @@
+/*
+ * 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/chassisControllerIntegrated.hpp"
+#include "okapi/api/chassis/controller/odomChassisController.hpp"
+#include "okapi/api/chassis/model/skidSteerModel.hpp"
+#include "okapi/api/odometry/odometry.hpp"
+#include <memory>
+
+namespace okapi {
+class DefaultOdomChassisController : public OdomChassisController {
+  public:
+  /**
+   * Odometry based chassis controller that moves using a separately constructed chassis controller.
+   * Spins up a task at the default priority plus 1 for odometry when constructed.
+   *
+   * Moves the robot around in the odom frame. Instead of telling the robot to drive forward or
+   * turn some amount, you instead tell it to drive to a specific point on the field or turn to
+   * a specific angle, relative to its starting position.
+   *
+   * @param itimeUtil The TimeUtil.
+   * @param iodometry The odometry to read state estimates from.
+   * @param icontroller The chassis controller to delegate to.
+   * @param imode The new default StateMode used to interpret target points and query the Odometry
+   * state.
+   * @param imoveThreshold minimum length movement (smaller movements will be skipped)
+   * @param iturnThreshold minimum angle turn (smaller turns will be skipped)
+   * @param ilogger The logger this instance will log to.
+   */
+  DefaultOdomChassisController(const TimeUtil &itimeUtil,
+                               std::shared_ptr<Odometry> iodometry,
+                               std::shared_ptr<ChassisController> icontroller,
+                               const StateMode &imode = StateMode::FRAME_TRANSFORMATION,
+                               QLength imoveThreshold = 0_mm,
+                               QAngle iturnThreshold = 0_deg,
+                               std::shared_ptr<Logger> ilogger = Logger::getDefaultLogger());
+
+  DefaultOdomChassisController(const DefaultOdomChassisController &) = delete;
+  DefaultOdomChassisController(DefaultOdomChassisController &&other) = delete;
+  DefaultOdomChassisController &operator=(const DefaultOdomChassisController &other) = delete;
+  DefaultOdomChassisController &operator=(DefaultOdomChassisController &&other) = delete;
+
+  /**
+   * Drives the robot straight to a point in the odom frame.
+   *
+   * @param ipoint The target point to navigate to.
+   * @param ibackwards Whether to drive to the target point backwards.
+   * @param ioffset An offset from the target point in the direction pointing towards the robot. The
+   * robot will stop this far away from the target point.
+   */
+  void driveToPoint(const Point &ipoint,
+                    bool ibackwards = false,
+                    const QLength &ioffset = 0_mm) override;
+
+  /**
+   * Turns the robot to face a point in the odom frame.
+   *
+   * @param ipoint The target point to turn to face.
+   */
+  void turnToPoint(const Point &ipoint) override;
+
+  /**
+   * @return The internal ChassisController.
+   */
+  std::shared_ptr<ChassisController> getChassisController();
+
+  /**
+   * @return The internal ChassisController.
+   */
+  ChassisController &chassisController();
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnToAngle(const QAngle &iangle) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveDistance(QLength itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveRaw(double itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveDistanceAsync(QLength itarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void moveRawAsync(double itarget) override;
+
+  /**
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle
+   */
+  void turnAngle(QAngle idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnRaw(double idegTarget) override;
+
+  /**
+   * Turns chassis to desired angle (turns in the direction of smallest angle)
+   * (ex. If current angle is 0 and target is 270, the chassis will turn -90 degrees)
+   * 
+   * @param idegTarget target angle 
+   */
+  void turnAngleAsync(QAngle idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void turnRawAsync(double idegTarget) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void setTurnsMirrored(bool ishouldMirror) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  bool isSettled() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void waitUntilSettled() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void stop() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  void setMaxVelocity(double imaxVelocity) override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  double getMaxVelocity() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  ChassisScales getChassisScales() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  AbstractMotor::GearsetRatioPair getGearsetRatioPair() const override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  std::shared_ptr<ChassisModel> getModel() override;
+
+  /**
+   * This delegates to the input ChassisController.
+   */
+  ChassisModel &model() override;
+
+  protected:
+  std::shared_ptr<Logger> logger;
+  std::shared_ptr<ChassisController> controller;
+
+  void waitForOdomTask();
+};
+} // namespace okapi