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

184 lines
5.9 KiB
C++
Raw Permalink Normal View History

2024-09-18 13:05:17 -04:00
/*
* 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